Using mesh and associated survey points in the tracking process

meshroom/aliceVision/SfMPoseInjecting.py

@@ -44,6 +44,12 @@ class SfMPoseInjecting(desc.AVCommandLineNode):
             value="EulerZXY",
             enabled=lambda node: pathlib.Path(node.posesFilename.value).suffix.lower() == ".json"
         ),
+        desc.BoolParam(
+            name="lockPoses",
+            label="Lock Injected Poses",
+            description="Do we lock the pose parameters for future refinement ?",
+            value=False,
+        ),
         desc.ChoiceParam(
             name="verboseLevel",
             label="Verbose Level",

src/aliceVision/__init__.py

@@ -20,6 +20,10 @@
         os.add_dll_directory(p)
 
 #Enforce loading order
+from . import system
 from . import geometry
 from . import sfmData
 from . import sfmDataIO
+
+#initialize logger
+system.initialize_alicevision_logger("info")

src/aliceVision/mesh/MeshIntersection.cpp

@@ -126,5 +126,38 @@ bool MeshIntersection::pickPointAndNormal(Vec3 & point, Vec3 & normal, const cam
     return true;
 }
 
+bool MeshIntersection::getPointAndNormal(Vec3 & point, Vec3 & normal, const Vec3 & origin, const Vec3 & direction)
+{
+    //Create geogram ray from alicevision ray
+    GEO::Ray ray;
+
+    ray.origin.x = origin.x();
+    ray.origin.y = -origin.y();
+    ray.origin.z = -origin.z();
+    ray.direction.x = direction.x();
+    ray.direction.y = -direction.y();
+    ray.direction.z = -direction.z();
+
+    GEO::MeshFacetsAABB::Intersection intersection;
+    if (!_aabb.ray_nearest_intersection(ray, intersection)) 
+    {
+        return false;
+    }
+
+    const GEO::vec3 p = ray.origin + intersection.t * ray.direction;
+
+    point.x() = p.x;
+    point.y() = -p.y;
+    point.z() = -p.z;
+
+    const GEO::vec3 n = GEO::normalize(intersection.N);
+
+    normal.x() = n.x;
+    normal.y() = -n.y;
+    normal.z() = -n.z;
+
+    return true;
+}
+
 }
 }

src/aliceVision/mesh/MeshIntersection.hpp

@@ -58,6 +58,16 @@ class MeshIntersection
     */
     bool pickPointAndNormal(Vec3 & point, Vec3 & normal, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords);
 
+    /**
+     * @brief get a point and get its normal on the mesh given 3D ray
+     * @param point the output measured point
+     * @param normal the output measured normal
+     * @param origin the ray origin
+     * @param direction the ray direction
+     * @return true if the ray intersects the mesh.
+    */
+    bool getPointAndNormal(Vec3 & point, Vec3 & normal, const Vec3 & origin, const Vec3 & direction);
+
 private:
     GEO::Mesh _mesh;
     GEO::MeshFacetsAABB _aabb;

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

@@ -12,9 +12,11 @@
 #include <aliceVision/sfm/bundle/costfunctions/constraint2d.hpp>
 #include <aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp>
 #include <aliceVision/sfm/bundle/costfunctions/projection.hpp>
+#include <aliceVision/sfm/bundle/costfunctions/projectionMesh.hpp>
 #include <aliceVision/sfm/bundle/costfunctions/rotationPrior.hpp>
 #include <aliceVision/sfm/bundle/costfunctions/temporalConstraint.hpp>
 #include <aliceVision/sfm/bundle/costfunctions/depth.hpp>
+#include <aliceVision/sfm/bundle/costfunctions/survey.hpp>
 #include <aliceVision/sfm/bundle/manifolds/intrinsics.hpp>
 #include <aliceVision/sfm/utils/poseFilter.hpp>
 #include <aliceVision/sfm/utils/statistics.hpp>
@@ -537,22 +539,44 @@
             continue;
         }
 
-        std::array<double, 3>& landmarkBlock = _landmarksBlocks[landmarkId];
+        std::array<double, 3> & landmarkBlock = _landmarksBlocks[landmarkId];
         for (std::size_t i = 0; i < 3; ++i)
         {
             landmarkBlock.at(i) = landmark.getX()(Eigen::Index(i));
         }
 
         //If the landmark has a referenceViewIndex set, then retrieve the reference pose
+        int sizeLandmark = 3;
         double * referencePoseBlockPtr = nullptr;
+
+
         if (landmark.getReferenceViewIndex() != UndefinedIndexT)
         {
+            // Store landmark in reference view geometric frame
             const sfmData::View& refview = sfmData.getView(landmark.getReferenceViewIndex());
+            const geometry::Pose3 refPose = sfmData.getPose(refview).getTransform();
+            Vec3 refX = refPose(landmark.getX());
+
+            // If there is a point fetcher, the landmark is only a bearing vector
+            if (landmark.getPointFetcher())
+            {
+                refX.x() /= refX.z();
+                refX.y() /= refX.z();
+                sizeLandmark = 2;
+            }
+
+            // Note: landmarkBlock[2] is only used when sizeLandmark == 3 (full 3D point).
+            // When sizeLandmark == 2 (bearing vector with point fetcher), this value is ignored
+            // but is set here for completeness and potential diagnostics.
+            landmarkBlock[0] = refX.x();
+            landmarkBlock[1] = refX.y();
+            landmarkBlock[2] = refX.z();
+
             referencePoseBlockPtr = _posesBlocks.at(refview.getPoseId()).data();
         }
 
         double* landmarkBlockPtr = landmarkBlock.data();
-        problem.AddParameterBlock(landmarkBlockPtr, 3);
+        problem.AddParameterBlock(landmarkBlockPtr, sizeLandmark);
 
         double* fakeDistortionBlockPtr = &_fakeDistortionBlock;
 
@@ -621,19 +645,20 @@
             else if (referencePoseBlockPtr != nullptr)
             {
                 bool samePose = (referencePoseBlockPtr == poseBlockPtr);
-                ceres::CostFunction* costFunction = ProjectionRelativeErrorFunctor::createCostFunction(intrinsic, observation, samePose);
+                ceres::CostFunction* costFunction = ProjectionMeshErrorFunctor::createCostFunction(intrinsic, observation, landmark.getPointFetcher(), samePose);
 
                 std::vector<double*> params;
                 params.push_back(intrinsicBlockPtr);
                 params.push_back(distortionBlockPtr);
+                params.push_back(poseBlockPtr);
                 if (!samePose)
                 {
-                    params.push_back(poseBlockPtr);
                     params.push_back(referencePoseBlockPtr);
                 }
                 params.push_back(landmarkBlockPtr);
 
-                problem.AddResidualBlock(costFunction, lossFunction, params);
+                ceres::ResidualBlockId blockId = problem.AddResidualBlock(costFunction, lossFunction, params);
+                blockIds.push_back(blockId);
             }
             else
             {
@@ -662,7 +687,7 @@
                 blockIds.push_back(blockId);
             }
 
-            if (!refineStructure || landmark.getState() == EEstimatorParameterState::CONSTANT || landmark.isPrecise())
+            if (!refineStructure || landmark.getState() == EEstimatorParameterState::CONSTANT || landmark.isLocked())
             {
                 // set the whole landmark parameter block as constant.
                 _statistics.addState(EParameter::LANDMARK, EEstimatorParameterState::CONSTANT);
@@ -685,14 +710,33 @@
         double* fakeDistortionBlockPtr = &_fakeDistortionBlock;
 
         const sfmData::View& view = sfmData.getView(idView);
+        if (!sfmData.isPoseAndIntrinsicDefined(view))
+        {
+            continue;
+        }
+
         const IndexT intrinsicId = view.getIntrinsicId();
+        const IndexT poseId = view.getPoseId();
+
+        // Do not add surveys if the pose is not to be estimated
+        if (_posesBlocks.find(poseId) == _posesBlocks.end())
+        {
+            continue;
+        }
+
+        // Do not add surveys if the intrinsic is not to be estimated
+        if (_intrinsicsBlocks.find(intrinsicId) == _intrinsicsBlocks.end())
+        {
+            continue;
+        }
 
         // each residual block takes a point and a camera as input and outputs a 2
         // dimensional residual. Internally, the cost function stores the observed
         // image location and compares the reprojection against the observation.
         const auto& pose = sfmData.getPose(view);
 
         // needed parameters to create a residual block (K, pose)
+
         double* poseBlockPtr = _posesBlocks.at(view.getPoseId()).data();
         double* intrinsicBlockPtr = _intrinsicsBlocks.at(intrinsicId).data();
         const std::shared_ptr<IntrinsicBase> intrinsic = _intrinsicObjects[intrinsicId];
@@ -716,7 +760,7 @@
         {
             sfmData::Observation observation(spoint.survey, 0, 1.0);
             ceres::CostFunction* costFunction =
-                    ProjectionSurveyErrorFunctor::createCostFunction(intrinsic, spoint.point3d, observation);
+                    SurveyErrorFunctor::createCostFunction(intrinsic, spoint.point3d, spoint.residual, observation, 1.0);
 
             std::vector<double*> params;
             params.push_back(intrinsicBlockPtr);
@@ -980,100 +1024,133 @@
     _linearSolverOrdering.Clear();
 }
 
 void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefineOptions refineOptions) const
 {
     const bool refinePoses = (refineOptions & REFINE_ROTATION) || (refineOptions & REFINE_TRANSLATION);
     const bool refineIntrinsicsOpticalCenter =
       (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS) || (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA);
     const bool refineIntrinsics =
       (refineOptions & REFINE_INTRINSICS_FOCAL) || (refineOptions & REFINE_INTRINSICS_DISTORTION) || refineIntrinsicsOpticalCenter;
     const bool refineStructure = refineOptions & REFINE_STRUCTURE;
 
     // update camera poses with refined data
     if (refinePoses)
     {
         // absolute poses
         for (auto & [poseId, pose] : sfmData.getPoses().valueRange())
         {
             // do not update a camera pose set as Ignored or Constant in the Local strategy
             if (pose.getState() != EEstimatorParameterState::REFINED)
             {
                 continue;
             }
 
             const std::array<double, 6> & poseBlock = _posesBlocks.at(poseId);
             pose.updateFromEstimator(poseBlock);
         }
 
         // rig sub-poses
         for (const auto& rigIt : _rigBlocks)
         {
             sfmData::Rig& rig = sfmData.getRigs().at(rigIt.first);
 
             for (const auto& subPoseit : rigIt.second)
             {
                 sfmData::RigSubPose& subPose = rig.getSubPose(subPoseit.first);
                 const std::array<double, 6>& subPoseBlock = subPoseit.second;
 
                 Mat3 R_refined;
                 ceres::AngleAxisToRotationMatrix(subPoseBlock.data(), R_refined.data());
                 const Vec3 t_refined(subPoseBlock.at(3), subPoseBlock.at(4), subPoseBlock.at(5));
 
                 // update the sub-pose
                 subPose.pose = poseFromRT(R_refined, t_refined);
             }
         }
     }
 
     // update camera intrinsics with refined data
     if (refineIntrinsics)
     {
         for (const auto& [idIntrinsic, block] : _intrinsicsBlocks)
         {
             const auto& intrinsic = sfmData.getIntrinsicSharedPtr(idIntrinsic);
 
             // do not update a camera pose set as Ignored or Constant in the Local strategy
             if (intrinsic->getState() != EEstimatorParameterState::REFINED)
             {
                 continue;
             }
 
             sfmData.getIntrinsics().at(idIntrinsic)->updateFromParams(block);
         }
 
         for (const auto& [idIntrinsic, distortionBlock]: _distortionsBlocks)
         {
             auto intrinsic = sfmData.getIntrinsicSharedPtr(idIntrinsic);
 
             // do not update a camera pose set as Ignored or Constant in the Local strategy
             if (intrinsic->getState() != EEstimatorParameterState::REFINED)
             {
                 continue;
             }
 
             auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
             if (isod)
             {
                 auto distortion = isod->getDistortion();
                 if (distortion)
                 {
                     distortion->setParameters(distortionBlock);
                 }
             }
         }
     }
 
     // update landmarks
     if (refineStructure)
     {
         for (const auto& [idLandmark, block] : _landmarksBlocks)
         {
             sfmData::Landmark& landmark = sfmData.getLandmarks().at(idLandmark);
-            landmark.updateFromEstimator(block);
+
+            std::array<double, 3> lblock = block;
+
+            if (landmark.getReferenceViewIndex() != UndefinedIndexT)
+            {
+                if (landmark.getPointFetcher())
+                {
+                    const sfmData::View& refview = sfmData.getView(landmark.getReferenceViewIndex());
+                    const geometry::Pose3 refPose = sfmData.getPose(refview).getTransform();
+
+                    const Vec3 origin = refPose.center();
+
+                    Vec3 rdir;
+                    rdir.x() = block[0];
+                    rdir.y() = block[1];
+                    rdir.z() = 1.0;
+
+                    rdir = rdir / rdir.norm();
+                    const Vec3 wdir = refPose.rotation().transpose() * rdir;
+
+                    Vec3 point, normal;
+                    if (!landmark.getPointFetcher()->getPointAndNormal(point, normal, origin, wdir))
+                    {
+                        ALICEVISION_LOG_DEBUG("A landmark wrongly intersect the mesh.");
+                        continue;
+                    }
+
+                    lblock[0] = point.x();
+                    lblock[1] = point.y();
+                    lblock[2] = point.z();
+                }
+            }
+
+            landmark.updateFromEstimator(lblock);
         }
     }
 }
 
 void BundleAdjustmentCeres::createJacobian(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::CRSMatrix& jacobian)
 {
     std::vector<ceres::ResidualBlockId> landmarksBlockIds;
@@ -1096,6 +1173,39 @@
     problem.Evaluate(evalOpt, &cost, NULL, NULL, &jacobian);
 }
 
+void BundleAdjustmentCeres::surveyInfos(const sfmData::SfMData & sfmData) const
+{
+    if (sfmData.getSurveyPoints().size() == 0)
+    {
+        return;
+    }
+
+    ALICEVISION_LOG_INFO("Surveys after minimization:");
+    for (const auto & [idView, surveys]: sfmData.getSurveyPoints())
+    {
+        const sfmData::View & view = sfmData.getView(idView);
+        if (!sfmData.isPoseAndIntrinsicDefined(view))
+        {
+            continue;
+        } 
+
+        const auto & intrinsic = sfmData.getIntrinsic(view.getIntrinsicId());
+        const auto pose = sfmData.getPose(view);
+
+        ALICEVISION_LOG_INFO("View " << idView << ":");
+        for (const auto & survey : surveys)
+        {
+            Vec2 obs = intrinsic.transformProject(pose.getTransform(), survey.point3d.homogeneous(), true);
+
+            double ex = std::abs(obs.x() - survey.survey.x());
+            double ey = std::abs(obs.y() - survey.survey.y());
+            double rx = std::abs(survey.residual.x());
+            double ry = std::abs(survey.residual.y());
+            ALICEVISION_LOG_INFO("Surveyed : [" << ex << ", " << ey << "], Current ["<< rx <<", "<< ry <<"]");
+        }
+    }
+}
+
 bool BundleAdjustmentCeres::adjust(sfmData::SfMData& sfmData, ERefineOptions refineOptions)
 {
     ALICEVISION_LOG_INFO("BundleAdjustmentCeres::adjust start");
@@ -1147,7 +1257,9 @@
 
     // print summary
     if (_ceresOptions.summary)
+    {
         ALICEVISION_LOG_INFO(summary.FullReport());
+    }
 
     // solution is not usable
     if (!summary.IsSolutionUsable())
@@ -1159,6 +1271,9 @@
     // update input sfmData with the solution
     updateFromSolution(sfmData, refineOptions);
 
+    // Info from surveys
+    surveyInfos(sfmData);
+
     // store some statistics from the summary
     _statistics.time = summary.total_time_in_seconds;
     _statistics.nbSuccessfullIterations = summary.num_successful_steps;

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp

@@ -238,6 +238,12 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback
      */
     void updateFromSolution(sfmData::SfMData& sfmData, ERefineOptions refineOptions) const;
 
+    /**
+    * @brief Print current state of surveys
+    * @param[in] sfmData the current sfmData to get infos from
+    */
+    void surveyInfos(const sfmData::SfMData & sfmData) const;
+
     // private members
 
     /// user Ceres options to use in the solver