Apply distance weighting for observations

meshroom/aliceVision/SfmExpanding.py

@@ -286,6 +286,12 @@ class SfMExpanding(desc.AVCommandLineNode):
             description="Bundle adjustment will try to optimize the landmarks positions.",
             value=True,
         ),
+        desc.BoolParam(
+            name="enableObservationsWeighting",
+            label="Enable observations weighting",
+            description="Enable observations weighting to reduce impact of regions with high point density.",
+            value=False,
+        ),
         desc.IntParam(
             name="minNbCamerasToRefinePrincipalPoint",
             label="Min Nb Cameras To Refine Principal Point",

src/aliceVision/sfm/CMakeLists.txt

@@ -81,6 +81,7 @@ set(sfm_files_sources
     pipeline/expanding/SfmTriangulation.cpp
     pipeline/expanding/SfmResection.cpp
     pipeline/expanding/SfmBundle.cpp
+    pipeline/expanding/DistanceWeighting.cpp
     pipeline/expanding/ExpansionHistory.cpp
     pipeline/expanding/ExpansionChunk.cpp
     pipeline/expanding/ExpansionIteration.cpp
@@ -136,6 +137,7 @@ alicevision_add_library(aliceVision_sfm
         Boost::boost
     PRIVATE_LINKS
         ${LEMON_LIBRARY}
+        nanoflann::nanoflann
 )
 
 # Unit tests

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

@@ -523,7 +523,8 @@
 
     // 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();
+    ceres::LossFunction * lossFunction = _ceresOptions.lossFunction.get();
+
 
     // build the residual blocks corresponding to the track observations
     for (const auto& landmarkPair : sfmData.getLandmarks())
@@ -625,6 +626,12 @@
                 _linearSolverOrdering.AddElementToGroup(distortionBlockPtr, 2);
             }
 
+            ceres::LossFunction * weightedLossFunction = lossFunction;
+            if (observation.getWeight() != 1.0)
+            {
+                weightedLossFunction = new ceres::ScaledLoss(lossFunction, observation.getWeight(), ceres::TAKE_OWNERSHIP);
+            }
+
             if (view.isPartOfRig() && !view.isPoseIndependant())
             {
                 ceres::CostFunction* costFunction = ProjectionErrorFunctor::createCostFunction(intrinsic, observation);
@@ -639,7 +646,7 @@
                 params.push_back(rigBlockPtr);
                 params.push_back(landmarkBlockPtr);
 
-                ceres::ResidualBlockId blockId = problem.AddResidualBlock(costFunction, lossFunction, params);
+                ceres::ResidualBlockId blockId = problem.AddResidualBlock(costFunction, weightedLossFunction, params);
                 blockIds.push_back(blockId);
             }
             else if (referencePoseBlockPtr != nullptr)
@@ -657,7 +664,7 @@
                 }
                 params.push_back(landmarkBlockPtr);
 
-                ceres::ResidualBlockId blockId = problem.AddResidualBlock(costFunction, lossFunction, params);
+                ceres::ResidualBlockId blockId = problem.AddResidualBlock(costFunction, weightedLossFunction, params);
                 blockIds.push_back(blockId);
             }
             else
@@ -670,7 +677,7 @@
                 params.push_back(poseBlockPtr);
                 params.push_back(landmarkBlockPtr);
 
-                ceres::ResidualBlockId blockId = problem.AddResidualBlock(costFunction, lossFunction, params);
+                ceres::ResidualBlockId blockId = problem.AddResidualBlock(costFunction, weightedLossFunction, params);
                 blockIds.push_back(blockId);
             }
 

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

@@ -0,0 +1,130 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2026 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/.
+
+#include <aliceVision/sfm/pipeline/expanding/DistanceWeighting.hpp>
+#include "nanoflann.hpp"
+
+#include <algorithm>
+#include <numeric>
+#include <cmath>
+
+namespace aliceVision {
+namespace sfm {
+
+struct PointCloud2D
+{
+    std::vector<sfmData::Observation *> pts;
+
+    // ── mandatory nanoflann interface ──────────
+
+    // Total number of points
+    inline size_t kdtree_get_point_count() const { return pts.size(); }
+
+    // Value of the d-th coordinate of the i-th point
+    inline double kdtree_get_pt(const size_t idx, const size_t dim) const
+    {
+        return dim == 0 ? pts[idx]->getX() : pts[idx]->getY();
+    }
+
+    // Optional bounding-box computation (return false = let nanoflann compute it)
+    template <class BBOX>
+    bool kdtree_get_bbox(BBOX&) const { return false; }
+};
+
+bool weightObservationsFromDistance(sfmData::SfMData & sfmData, size_t neighboorRank, double ratioDefaultArea)
+{
+    const int K = static_cast<int>(neighboorRank);
+
+    sfmData::Landmarks & landmarks = sfmData.getLandmarks();
+
+    std::map<IndexT, PointCloud2D> perViewObservations;
+
+    // Make list of landmark per view
+    for (auto & [_, landmark] : landmarks)
+    {
+        for (auto & [idView, obs] : landmark.getObservations())
+        {
+            perViewObservations[idView].pts.push_back(&obs);
+        }
+    }
+
+    // Loop per view
+    for (auto & [idView, pointCloud] : perViewObservations)
+    {
+        int N = pointCloud.pts.size();
+
+        if (N == 0)
+        {
+            continue;
+        }
+
+        const sfmData::View & v = sfmData.getView(idView);
+        const double viewArea = v.getImage().getWidth() * v.getImage().getHeight();
+
+
+        // average area per feature
+        const double featArea = viewArea / N;
+        const double minSqDist = 1.0;
+        const double maxSqDist = ratioDefaultArea * featArea;
+
+        if (N < 10)
+        {
+            for (size_t i = 0; i < N; ++i)
+            {
+                pointCloud.pts[i]->setWeight(1.0);
+            }
+            continue;
+        }    
+
+        using KDTree2D = nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, PointCloud2D>, PointCloud2D, 2>;
+
+        KDTree2D index(2, pointCloud, nanoflann::KDTreeSingleIndexAdaptorParams(10));
+        index.buildIndex();
+
+        std::vector<double> knnSquaredDistance(N);
+
+        // Find the distance to the Kth nearest neighbor
+        #pragma omp parallel
+        {
+            std::vector<unsigned int> indices(K);
+            std::vector<double> sq_dists(K);
+
+            #pragma omp for schedule(static)
+            for (size_t i = 0; i < N; ++i)
+            {
+                const double query[2] = { pointCloud.pts[i]->getX(), pointCloud.pts[i]->getY() };
+                index.knnSearch(query, K, indices.data(), sq_dists.data());
+                knnSquaredDistance[i] = std::clamp(sq_dists[K - 1], minSqDist, maxSqDist);
+            }
+        }
+
+
+
+        // Normalize so the mean is 1 (overall influence is 1).what is
+        const double meanNormalizedDistance = std::accumulate(knnSquaredDistance.begin(), knnSquaredDistance.end(), 0.0) / static_cast<double>(N);
+
+        // Fallback to uniform weighting if the stats are incorrect
+        if (meanNormalizedDistance < 1e-12)
+        {
+            for (size_t i = 0; i < N; ++i)
+            {
+                pointCloud.pts[i]->setWeight(1.0);
+            }
+            continue;
+        }
+
+        for (size_t i = 0; i < N; ++i)
+        {
+            knnSquaredDistance[i] /= meanNormalizedDistance;
+            pointCloud.pts[i]->setWeight(knnSquaredDistance[i]);
+        }        
+    }
+
+    return true;
+}
+
+}
+}

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

@@ -0,0 +1,32 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2026 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/sfmData/SfMData.hpp>
+
+namespace aliceVision {
+namespace sfm {
+
+/**
+ * @brief Weight observations based on their spatial distribution in the image.
+ * Points that are in sparse areas (far from their neighbors) get a higher weight,
+ * while points in dense clusters get a lower weight. This helps to balance the
+ * influence of features across the image.
+ *
+ * The weight is calculated based on the squared distance to the K-th nearest neighbor (neighboorRank),
+ * clamped to a maximum area (ratioDefaultArea * averageFeatureArea).
+ * Weights are normalized per view so that their mean is 1.0.
+ *
+ * @param[in,out] sfmData The SfM data containing views and landmarks to be weighted.
+ * @param[in] neighboorRank The rank (K) of the neighbor to use for distance calculation.
+ * @param[in] ratioDefaultArea Multiplier for clamping the maximum area to consider for a point.
+ * @return True if successful.
+ */
+bool weightObservationsFromDistance(sfmData::SfMData & sfmData, size_t neighboorRank, double ratioDefaultArea);
+
+}
+}

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

@@ -9,7 +9,9 @@
 #include <aliceVision/system/Logger.hpp>
 #include <aliceVision/sfm/sfmFilters.hpp>
 #include <aliceVision/sfm/utils/poseFilter.hpp>
+#include <aliceVision/sfm/sfmStatistics.hpp>
 #include <aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp>
+#include <aliceVision/sfm/pipeline/expanding/DistanceWeighting.hpp>
 
 namespace aliceVision {
 namespace sfm {
@@ -51,6 +53,11 @@ bool SfmBundle::process(sfmData::SfMData & sfmData, const track::TracksHandler &
     //Repeat until nothing change
     do
     {
+        // Compute statistics on residuals
+        double mean, median;
+        computeResidualsMeanMedian(sfmData, mean, median);
+        ALICEVISION_LOG_INFO("SfmBundle statistics before minimization - mean " << mean << ", median " << median);
+
         if (!initializeIteration(sfmData, tracksHandler, viewIds))
         {
             return false;
@@ -61,6 +68,9 @@ bool SfmBundle::process(sfmData::SfMData & sfmData, const track::TracksHandler &
         {
             return false;
         }
+
+        computeResidualsMeanMedian(sfmData, mean, median);
+        ALICEVISION_LOG_INFO("SfmBundle statistics after minimization - mean " << mean << ", median " << median);
     }
     while (cleanup(sfmData));
 
@@ -121,6 +131,11 @@ bool SfmBundle::initializeIteration(sfmData::SfMData & sfmData, const track::Tra
         sfmData.resetParameterStates();
     }
 
+    if (_enableObservationsWeighting)
+    {
+        weightObservationsFromDistance(sfmData, 15, 5.0);
+    }
+
     return true;
 }
 

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

@@ -87,6 +87,15 @@ class SfmBundle
         _isStructureRefinementEnabled = flag;
     }
 
+    /**
+     * @brief Set whether to enable weighting of observations
+     * @param flag true to enable observations weighting, false to disable it
+    */
+    void setIsObservationsWeightingEnabled(bool flag)
+    {
+        _enableObservationsWeighting = flag;
+    }
+
     /**
      * @brief Get the number of valid points per pose required
      * @return the threshold used to discriminate a valid pose
@@ -141,6 +150,7 @@ class SfmBundle
     size_t _LBAGraphDistanceLimit = 1;
     size_t _LBAMinNbOfMatches = 50;
     bool _isStructureRefinementEnabled = true;
+    bool _enableObservationsWeighting = false;
 };
 
 } // namespace sfm

src/aliceVision/sfm/sfmStatistics.cpp

@@ -73,6 +73,54 @@ void computeResidualsHistogram(const sfmData::SfMData& sfmData,
     }
 }
 
+void computeResidualsMeanMedian(const sfmData::SfMData& sfmData,
+                            double & mean, 
+                            double & median,
+                            const std::set<IndexT>& specificViews)
+{
+    mean = 0;
+    median = 0;
+
+    if (sfmData.getLandmarks().empty())
+    {
+        return;
+    }
+
+    // Collect residuals for each observation
+    std::vector<double> vecResiduals;
+    vecResiduals.reserve(sfmData.getLandmarks().size());
+
+    for (const auto& track : sfmData.getLandmarks())
+    {
+        const aliceVision::sfmData::Observations& observations = track.second.getObservations();
+        for (const auto& obs : observations)
+        {
+            if (!specificViews.empty())
+            {
+                if (specificViews.count(obs.first) == 0)
+                {
+                    continue;
+                }
+            }
+
+            const sfmData::View& view = sfmData.getView(obs.first);
+            const aliceVision::geometry::Pose3 pose = sfmData.getPose(view).getTransform();
+            const aliceVision::camera::IntrinsicBase& intrinsic = sfmData.getIntrinsic(view.getIntrinsicId());
+            const Vec2 residual = intrinsic.residual(pose, track.second.getX().homogeneous(), obs.second.getCoordinates());
+            vecResiduals.push_back(residual.norm());
+        }
+    }
+
+    if (vecResiduals.empty())
+    {
+        return;
+    }
+
+    BoxStats<double> stats(vecResiduals.begin(), vecResiduals.end());
+    mean = stats.mean;
+    median = stats.median;
+}
+
 void computeObservationsLengthsHistogram(const sfmData::SfMData& sfmData,
                                          BoxStats<double>& outStats,
                                          int& overallNbObservations,

src/aliceVision/sfm/sfmStatistics.hpp

@@ -29,6 +29,18 @@ void computeResidualsHistogram(const sfmData::SfMData& sfmData,
                                utils::Histogram<double>* out_histogram,
                                const std::set<IndexT>& specificViews = std::set<IndexT>());
 
+/**
+ * @brief Compute histogram of residual values between landmarks and features in all the views specified
+ * @param[in] sfmData : scene containing the features and the landmarks
+ * @param[in] specificViews: Limit stats to specific views. If empty, compute stats for all views.
+ * @param[out] mean : mean of residuals
+ * @param[out] median : median of residuals
+ */
+void computeResidualsMeanMedian(const sfmData::SfMData& sfmData,
+                            double & mean,
+                            double & median,
+                            const std::set<IndexT>& specificViews = std::set<IndexT>());
+
 /**
  * @brief Compute histogram of observations lengths
  * @param[in] sfmData: containing the observations