Use constraint points in expansion

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

@@ -0,0 +1,38 @@
+// 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 Vec3 & normal, const Vec3 & point)        
+    : _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] = 100.0 * (distance - _constraintDistance);
+
+        return true;
+    }
+
+    Vec3 _normal;
+    double _constraintDistance;
+};
+
+}  // namespace sfm
+}  // namespace aliceVision

src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp

@@ -86,6 +86,11 @@ bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHand
         return false;
     }
 
+    if (_pointFetcherHandler)
+    {
+        setConstraints(sfmData, tracksHandler, validViewIds);
+    }
+
     if (_historyHandler)
     {
         _historyHandler->saveState(sfmData);
@@ -158,6 +163,125 @@ void ExpansionChunk::addPose(sfmData::SfMData & sfmData, IndexT viewId, const Ei
     sfmData.setPose(v, cpose);
 }
 
+void ExpansionChunk::setConstraints(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set<IndexT> & viewIds)
+{
+    ALICEVISION_LOG_INFO("ExpansionChunk::setConstraints start");
+    const track::TracksMap & tracks = tracksHandler.getAllTracks();
+    const track::TracksPerView & tracksPerView = tracksHandler.getTracksPerView();
+
+    const sfmData::Landmarks & landmarks = sfmData.getLandmarks();
+    sfmData::ConstraintsPoint & constraints = sfmData.getConstraintsPoint();
+
+    std::map<IndexT, std::vector<std::pair<Vec3, Vec3>>> infoPerLandmark;
+
+    // Fetch all points and normals and store them for further voting
+    for (const auto & viewId: sfmData.getValidViews())
+    {
+        const sfmData::View & v = sfmData.getView(viewId);
+        const sfmData::CameraPose & cp = sfmData.getAbsolutePose(v.getPoseId());
+        const camera::IntrinsicBase & intrinsics = *sfmData.getIntrinsics().at(v.getIntrinsicId());
+
+        _pointFetcherHandler->setPose(cp.getTransform());
+
+        const auto & trackIds = tracksPerView.at(viewId);
+        for (const auto trackId : trackIds)
+        {
+            const track::Track & track = tracks.at(trackId);
+            const track::TrackItem & trackItem = track.featPerView.at(viewId);
+
+            if (landmarks.find(trackId) == landmarks.end())
+            {
+                continue;
+            }
+
+            Vec3 point, normal;
+            if (!_pointFetcherHandler->peekPointAndNormal(point, normal, intrinsics, trackItem.coords))
+            {
+                continue;
+            }
+
+            infoPerLandmark[trackId].push_back(std::make_pair(point, normal));
+        }
+    }
+
+    //Find the consensus
+    const double maxDist = 0.1;
+    const double maxDistLandmark = 1.0;
+    const double cosMaxAngle = cos(M_PI_4);
+
+    for (const auto & [trackId, vecInfo] : infoPerLandmark)
+    {
+        if (vecInfo.size() == 0)
+        {
+            continue;
+        }
+
+        int idBest = -1;
+        int countBest = -1;
+
+
+        //Consider each point
+        for (int idRef = 0; idRef < vecInfo.size(); idRef++)
+        {
+            const Vec3 & refpt = vecInfo[idRef].first;
+            const Vec3 & refnormal = vecInfo[idRef].second;
+
+            //Compare it with all other points
+            int count = 0;
+            for (int idCur = 0; idCur < vecInfo.size(); idCur++)
+            {
+                if (idCur == idRef)
+                {
+                    continue;
+                }
+
+                const Vec3 & curpt = vecInfo[idRef].first;
+                const Vec3 & curnormal = vecInfo[idRef].second;
+
+                double dist = (refpt - curpt).norm();
+                if (dist > maxDist) 
+                {
+                    continue;
+                }
+
+                if (curnormal.dot(refnormal) < cosMaxAngle)
+                {
+                    continue;
+                }
+
+                count++;
+            }
+
+            if (count > countBest)
+            {
+                idBest = idRef;
+                countBest = count;
+            }
+        }
+
+        const auto & landmark = landmarks.at(trackId);
+        const Vec3 point = vecInfo[idBest].first;
+        const Vec3 normal = vecInfo[idBest].second;
+
+        double dist = (point - landmark.X).norm();
+        if (dist > maxDistLandmark)
+        {
+            continue;
+        }
+
+        if (idBest < 0)
+        {
+            ALICEVISION_THROW_ERROR("Impossible value");
+        }
+
+        sfmData::ConstraintPoint cp(trackId, normal, point);
+        constraints[trackId] = cp;
+    }
+
+    ALICEVISION_LOG_INFO("ExpansionChunk::setConstraints added " << constraints.size() << " constraints");
+    ALICEVISION_LOG_INFO("ExpansionChunk::setConstraints end");
+}
+
 } // namespace sfm
 } // namespace aliceVision
 

src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp

@@ -11,6 +11,7 @@
 #include <aliceVision/sfmData/SfMData.hpp>
 #include <aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp>
 #include <aliceVision/sfm/pipeline/expanding/SfmBundle.hpp>
+#include <aliceVision/sfm/pipeline/expanding/PointFetcher.hpp>
 
 namespace aliceVision {
 namespace sfm {
@@ -19,7 +20,7 @@ class ExpansionChunk
 {
 public:
     using uptr = std::unique_ptr<ExpansionChunk>;
-
+    
 public:
 
     /**
@@ -51,6 +52,15 @@ class ExpansionChunk
         _historyHandler = expansionHistory;
     }
 
+    /**
+     * brief setup the point fetcher handler
+     * @param pointFetcher a unique ptr. the Ownership will be taken
+    */
+    void setPointFetcherHandler(PointFetcher::uptr & pointFetcherHandler)
+    {
+        _pointFetcherHandler = std::move(pointFetcherHandler);
+    }
+
     void setResectionMaxIterations(size_t maxIterations)
     {
         _resectionIterations = maxIterations;
@@ -106,9 +116,18 @@ class ExpansionChunk
      */
     bool triangulate(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set<IndexT> & viewIds);
 
+    /**
+     * @brief Add constraints on points
+     * @param sfmData the object to update
+     * @param tracks all tracks of the scene as a map {trackId, track}
+     * @param viewIds the set of views to process 
+    */
+    void setConstraints(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set<IndexT> & viewIds);
+
 private:
     SfmBundle::uptr _bundleHandler;
     ExpansionHistory::sptr _historyHandler;
+    PointFetcher::uptr _pointFetcherHandler;
 
 private:    
     size_t _resectionIterations = 1024;

src/aliceVision/sfm/pipeline/expanding/PointFetcher.hpp

@@ -0,0 +1,45 @@
+// 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/numeric/numeric.hpp>
+#include <aliceVision/camera/IntrinsicBase.hpp>
+
+namespace aliceVision 
+{
+namespace sfm
+{
+
+class PointFetcher
+{
+public:
+    using uptr = std::unique_ptr<PointFetcher>;
+
+public:
+    /**
+     * Set the pose of the camera
+     * @param the pose of the camera wrt some global coordinates frame
+    */
+    virtual void setPose(const geometry::Pose3 & pose) = 0;
+
+    /**
+     * @brief virtual method to get coordinates and normals of a pixel of an image
+     * @param point result point in some global coordinates frame
+     * @param normal result normal in some global coordinates frame
+     * @param pose pose of the camera wrt some global coordinates frame
+     * @param intrinsic the camera intrinsic object
+     * @param imageCoords the input image pixel coordinates in 2D.
+     * @return false on error 
+    */
+    virtual bool peekPointAndNormal(Vec3 & point, 
+                                Vec3 & normal, 
+                                const camera::IntrinsicBase & intrinsic, 
+                                const Vec2 & imageCoords) = 0;
+};
+
+}
+}

src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp

@@ -91,11 +91,12 @@ bool SfmResection::processView(
     }
 
     //Refine the pose
-    if (!internalRefinement(structure, observations, inliers, pose, intrinsic))
+    if (!internalRefinement(structure, observations, inliers, pose, intrinsic, errorMax))
     {
         ALICEVISION_LOG_INFO("SfmResection::processView internalRefinemanet failed " << viewId);
         return false;
     }
+
 
     updatedThreshold = errorMax;
     updatedPose = pose;
@@ -140,7 +141,8 @@ bool SfmResection::internalRefinement(
         const std::vector<Eigen::Vector2d> & observations,
         const std::vector<size_t> & inliers,
         Eigen::Matrix4d & pose, 
-        std::shared_ptr<camera::IntrinsicBase> & intrinsics)
+        std::shared_ptr<camera::IntrinsicBase> & intrinsics,
+        double & errorMax)
 {
     // Setup a tiny SfM scene with the corresponding 2D-3D data
     sfmData::SfMData tinyScene;
@@ -155,7 +157,7 @@ bool SfmResection::internalRefinement(
     // Intrinsics
     tinyScene.getIntrinsics().emplace(0, intrinsics);
 
-    const double unknownScale = 0.0;
+    const double unknownScale = 1.0;
 
     // structure data (2D-3D correspondences)
     for(std::size_t i = 0; i < inliers.size(); ++i)
@@ -179,8 +181,21 @@ bool SfmResection::internalRefinement(
 
     pose = tinyScene.getPose(*view).getTransform().getHomogeneous();
 
+    // Compute errorMax
+    errorMax = 0.0;
+    for(std::size_t i = 0; i < inliers.size(); ++i)
+    {
+        const std::size_t idx = inliers[i];
+
+        Vec2 est = intrinsics->transformProject(pose, structure[idx].homogeneous(), true);
+        Vec2 diff = observations[idx] - est;
+
+        errorMax = std::max(errorMax, diff.norm());
+    }
+
+
     return true;
 }
 
 } // namespace sfm
-} // namespace aliceVision
+} // namespace aliceVision

src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp

@@ -61,7 +61,8 @@ class SfmResection
             const std::vector<Eigen::Vector2d> & observations,
             const std::vector<size_t> & inliers,
             Eigen::Matrix4d & pose, 
-            std::shared_ptr<camera::IntrinsicBase> & intrinsics
+            std::shared_ptr<camera::IntrinsicBase> & intrinsics,
+            double & errorMax
         );
 
 private: