Merge pull request #1803 from alicevision/dev/sfmLidar

Use lidar as constraints in sfm

src/aliceVision/keyframe/KeyframeSelector.cpp

@@ -1252,6 +1252,8 @@ bool KeyframeSelector::writeSfMData(const std::string& mediaPath,
 
 bool KeyframeSelector::writeSfMDataFromSfMData(const std::string& mediaPath)
 {
+    auto& keyframesPoses = _outputSfmKeyframes.getPoses();
+
     auto& keyframesViews = _outputSfmKeyframes.getViews();
     auto& framesViews = _outputSfmFrames.getViews();
 
@@ -1292,6 +1294,18 @@ bool KeyframeSelector::writeSfMDataFromSfMData(const std::string& mediaPath)
     {
         viewId = views[i]->getViewId();
         intrinsicId = views[i]->getIntrinsicId();
+        IndexT poseId = views[i]->getPoseId();
+
+        bool hasPose = inputSfm.isPoseAndIntrinsicDefined(viewId);
+        if (hasPose)
+        {
+            ALICEVISION_LOG_INFO("A fully calibrated view (Id:" << viewId << ") is moved to keyframes");
+            _selectedFrames[i] = '1';
+
+            //Make sure to keep the pose
+            keyframesPoses.emplace(poseId, inputSfm.getPoses().at(poseId));
+        }
+
         if (_selectedFrames[i] == '1')
         {
             keyframesViews.emplace(viewId, views[i]);

src/aliceVision/mesh/MeshIntersection.cpp

@@ -28,7 +28,7 @@ bool MeshIntersection::initialize(const std::string & pathToModel)
     return true;
 }
 
-bool MeshIntersection::peekPoint(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords)
+bool MeshIntersection::pickPoint(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords)
 {
     const Vec3 posCamera = _pose.center();
     const Vec3 wdir = intrinsic.backprojectTransform(imageCoords, true, _pose, 1.0);
@@ -58,7 +58,7 @@ bool MeshIntersection::peekPoint(Vec3 & output, const camera::IntrinsicBase & in
     return true;
 }
 
-bool MeshIntersection::peekNormal(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords)
+bool MeshIntersection::pickNormal(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords)
 {
     const Vec3 posCamera = _pose.center();
     const Vec3 wdir = intrinsic.backprojectTransform(imageCoords, true, _pose, 1.0);
@@ -88,5 +88,41 @@ bool MeshIntersection::peekNormal(Vec3 & output, const camera::IntrinsicBase & i
     return true;
 }
 
+bool MeshIntersection::pickPointAndNormal(Vec3 & point, Vec3 & normal, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords)
+{
+    const Vec3 posCamera = _pose.center();
+    const Vec3 wdir = intrinsic.backprojectTransform(imageCoords, true, _pose, 1.0);
+    const Vec3 dir = (wdir - posCamera).normalized();
+
+    //Create geogram ray from alicevision ray
+    GEO::Ray ray;
+    ray.origin.x = posCamera.x();
+    ray.origin.y = -posCamera.y();
+    ray.origin.z = -posCamera.z();
+    ray.direction.x = dir.x();
+    ray.direction.y = -dir.y();
+    ray.direction.z = -dir.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

@@ -22,7 +22,7 @@ class MeshIntersection
     bool initialize(const std::string & pathToModel);
 
     /**
-     * @brief Update pose to use for peeking
+     * @brief Update pose to use for picking
      * @param pose transformation to use (in aliceVision standard form)
     */
     void setPose(const geometry::Pose3 & pose)
@@ -31,22 +31,32 @@ class MeshIntersection
     }
 
     /**
-     * @brief peek a point on the mesh given a input camera observation
+     * @brief pick a point on the mesh given a input camera observation
      * @param output the output measured point
      * @param intrinsic the camera intrinsics to use for ray computation
      * @param imageCoords the camera observation we want to use to estimate its 'depth'
-     * @return true if the ray intersect the mesh.
+     * @return true if the ray intersects the mesh.
     */
-    bool peekPoint(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords);
+    bool pickPoint(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords);
 
     /**
-     * @brief peek a point and get its normal on the mesh given a input camera observation
+     * @brief pick a point and get its normal on the mesh given a input camera observation
      * @param output the output measured normal
      * @param intrinsic the camera intrinsics to use for ray computation
      * @param imageCoords the camera observation we want to use to estimate its 'depth'
-     * @return true if the ray intersect the mesh.
+     * @return true if the ray intersects the mesh.
     */
-    bool peekNormal(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords);
+    bool pickNormal(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords);
+
+    /**
+     * @brief pick a point and get its normal on the mesh given a input camera observation
+     * @param point the output measured point
+     * @param normal the output measured normal
+     * @param intrinsic the camera intrinsics to use for ray computation
+     * @param imageCoords the camera observation we want to use to estimate its 'depth'
+     * @return true if the ray intersects the mesh.
+    */
+    bool pickPointAndNormal(Vec3 & point, Vec3 & normal, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords);
 
 private:
     GEO::Mesh _mesh;

src/aliceVision/sfm/CMakeLists.txt

@@ -24,6 +24,9 @@ set(sfm_files_headers
   pipeline/RelativePoseInfo.hpp
   pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.hpp
   pipeline/panorama/ReconstructionEngine_panorama.hpp
+  pipeline/bootstrapping/EstimateAngle.hpp
+  pipeline/bootstrapping/PairsScoring.hpp
+  pipeline/bootstrapping/Bootstrap.hpp
   pipeline/expanding/SfmTriangulation.hpp
   pipeline/expanding/SfmResection.hpp
   pipeline/expanding/SfmBundle.hpp
@@ -65,6 +68,9 @@ set(sfm_files_sources
   pipeline/RelativePoseInfo.cpp
   pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp
   pipeline/panorama/ReconstructionEngine_panorama.cpp
+  pipeline/bootstrapping/EstimateAngle.cpp
+  pipeline/bootstrapping/PairsScoring.cpp
+  pipeline/bootstrapping/Bootstrap.cpp
   pipeline/expanding/SfmTriangulation.cpp
   pipeline/expanding/SfmResection.cpp
   pipeline/expanding/SfmBundle.cpp

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

@@ -7,6 +7,7 @@
 
 #include <aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp>
 #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/rotationPrior.hpp>
 #include <aliceVision/sfm/bundle/manifolds/intrinsics.hpp>
@@ -86,6 +87,17 @@ ceres::CostFunction* createConstraintsCostFunctionFromIntrinsics(std::shared_ptr
     return costFunction;
 }
 
+ceres::CostFunction* createCostFunctionFromContraintPoint(const sfmData::Landmark & landmark, const Vec3 & normal)
+{
+    const double weight = 100.0;
+    auto costFunction = new ceres::DynamicAutoDiffCostFunction<ConstraintPointErrorFunctor>(new ConstraintPointErrorFunctor(weight, normal, landmark.X));
+
+    costFunction->AddParameterBlock(3);
+    costFunction->SetNumResiduals(1);
+
+    return costFunction;
+}
+
 void BundleAdjustmentCeres::CeresOptions::setDenseBA()
 {
     // default configuration use a DENSE representation
@@ -671,6 +683,33 @@ void BundleAdjustmentCeres::addConstraints2DToProblem(const sfmData::SfMData& sf
     }
 }
 
+void BundleAdjustmentCeres::addConstraintsPointToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem)
+{
+    // set a LossFunction to be less penalized by false measurements.
+    // note: set it to NULL if you don't want use a lossFunction.
+    ceres::LossFunction* lossFunction = _ceresOptions.lossFunction.get();
+
+    for (const auto& [landmarkId, constraint] : sfmData.getConstraintsPoint())
+    {
+        if (sfmData.getLandmarks().find(landmarkId) == sfmData.getLandmarks().end())
+        {
+            continue;
+        }
+
+        if (_landmarksBlocks.find(landmarkId) == _landmarksBlocks.end())
+        {
+            continue;
+        }
+
+        const sfmData::Landmark & l = sfmData.getLandmarks().at(landmarkId);
+        double * ldata = _landmarksBlocks.at(landmarkId).data();
+
+        ceres::CostFunction* costFunction = createCostFunctionFromContraintPoint(l, constraint.normal);
+
+        problem.AddResidualBlock(costFunction, lossFunction, ldata);
+    }
+}
+
 void BundleAdjustmentCeres::addRotationPriorsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem)
 {
     // set a LossFunction to be less penalized by false measurements.
@@ -718,6 +757,9 @@ void BundleAdjustmentCeres::createProblem(const sfmData::SfMData& sfmData, ERefi
     // add 2D constraints to the Ceres problem
     addConstraints2DToProblem(sfmData, refineOptions, problem);
 
+    // add 2D constraints to the Ceres problem
+    addConstraintsPointToProblem(sfmData, refineOptions, problem);
+
     // add rotation priors to the Ceres problem
     addRotationPriorsToProblem(sfmData, refineOptions, problem);
 }

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp

@@ -189,6 +189,14 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback
      */
     void addConstraints2DToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem);
 
+    /**
+     * @brief Create a residual block for each point constraints
+     * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics
+     * @param[in] refineOptions The chosen refine flag
+     * @param[out] problem The Ceres bundle adjustement problem
+     */
+    void addConstraintsPointToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem);
+
     /**
      * @brief Create a residual block for each rotation priors
      * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics

src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp

@@ -0,0 +1,39 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2025 AliceVision contributors.
+// This Source Code Form is subject to the terms of the Mozilla Public License,
+// v. 2.0. If a copy of the MPL was not distributed with this file,
+// You can obtain one at https://mozilla.org/MPL/2.0/.
+
+#pragma once
+
+#include <aliceVision/camera/camera.hpp>
+
+namespace aliceVision {
+namespace sfm {
+
+struct ConstraintPointErrorFunctor
+{
+    explicit ConstraintPointErrorFunctor(const double weight, const Vec3 & normal, const Vec3 & point)        
+    : _weight(weight), _normal(normal)
+    {        
+        _constraintDistance = _normal.dot(point);
+    }
+
+    template<typename T>
+    bool operator()(T const* const* parameters, T* residuals) const
+    {       
+        const T* parameter_point = parameters[0];
+
+        T distance = parameter_point[0] * _normal[0] + parameter_point[1] * _normal[1] + parameter_point[2] * _normal[2];
+        residuals[0] = _weight * (distance - _constraintDistance);
+
+        return true;
+    }
+
+    double _weight;
+    Vec3 _normal;
+    double _constraintDistance;
+};
+
+}  // namespace sfm
+}  // namespace aliceVision