Survey cost function update

Fabien Servant · Fabien Servant · commit 07d335973f4d · 2026-01-19T09:53:35.000+01:00
diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp
@@ -16,6 +16,7 @@
 #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>
@@ -707,14 +708,33 @@ void BundleAdjustmentCeres::addSurveyPointsToProblem(const sfmData::SfMData& sfm
         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];
@@ -738,7 +758,7 @@ void BundleAdjustmentCeres::addSurveyPointsToProblem(const sfmData::SfMData& sfm
         {
             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);
@@ -1146,6 +1166,39 @@ void BundleAdjustmentCeres::createJacobian(const sfmData::SfMData& sfmData, ERef
     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 minimzization:");
+    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.x());
+            ALICEVISION_LOG_INFO("Surveyed : [" << ex << ", " << ey << "], Current ["<< rx <<", "<< ry <<"]");
+        }
+    }
+}
+
 bool BundleAdjustmentCeres::adjust(sfmData::SfMData& sfmData, ERefineOptions refineOptions)
 {
     ALICEVISION_LOG_INFO("BundleAdjustmentCeres::adjust start");
@@ -1211,6 +1264,9 @@ bool BundleAdjustmentCeres::adjust(sfmData::SfMData& sfmData, ERefineOptions ref
     // 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;
diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp b/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
diff --git a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp
@@ -93,108 +93,6 @@ struct ProjectionSimpleErrorFunctor
     ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor;
 };
 
-
-struct ProjectionSurveyErrorFunctor
-{
-    explicit ProjectionSurveyErrorFunctor(const Vec3 & point, 
-                                            const sfmData::Observation& obs, 
-                                            const std::shared_ptr<camera::IntrinsicBase>& intrinsics)        
-    : _intrinsicFunctor(new CostIntrinsicsProject(obs, intrinsics)), _point(point)
-    {        
-    }
-
-    template<typename T>
-    T func(const T & input) const 
-    {
-        const T alpha = T(5.0);
-        const T coeff = T(0.05);
-
-        const T p1 = abs(alpha - T(2));
-        const T p2 = coeff * input * input;
-        
-        return (p1/alpha) * (pow(T(1) + p2/p1, alpha / T(2)) - T(1));
-    }
-
-    template<typename T>
-    bool operator()(T const* const* parameters, T* residuals) const
-    {       
-        const T* parameter_intrinsics = parameters[0];
-        const T* parameter_distortion = parameters[1];
-        const T* parameter_pose = parameters[2];
-        T parameter_point[3];
-        
-        parameter_point[0] = T(_point.x());
-        parameter_point[1] = T(_point.y());
-        parameter_point[2] = T(_point.z());
-
-        //--
-        // Apply external parameters (Pose)
-        //--
-        const T* cam_R = parameter_pose;
-        const T* cam_t = &parameter_pose[3];
-        
-        T transformedPoint[3];
-        // Rotate the point according the camera rotation
-        ceres::AngleAxisRotatePoint(cam_R, parameter_point, transformedPoint);
-
-        // Apply the camera translation
-        transformedPoint[0] += cam_t[0];
-        transformedPoint[1] += cam_t[1];
-        transformedPoint[2] += cam_t[2];
-
-        const T * innerParameters[3];
-        innerParameters[0] = parameter_intrinsics;
-        innerParameters[1] = parameter_distortion;
-        innerParameters[2] = transformedPoint;
-
-        if (!_intrinsicFunctor(innerParameters, residuals))
-        {
-            return false;
-        }
-
-        residuals[0] = func(residuals[0]);
-        residuals[1] = func(residuals[1]);
-
-        return true;
-    }
-
-    /**
-     * @brief Create the appropriate cost functor according the provided input camera intrinsic model
-     * @param[in] intrinsicPtr The intrinsic pointer
-     * @param[in] point the  reference point to compare to
-     * @param[in] observation The corresponding observation
-     * @return cost functor
-     */
-    inline static ceres::CostFunction* createCostFunction(
-                                            const std::shared_ptr<camera::IntrinsicBase> intrinsic, 
-                                            const Vec3 & point,
-                                            const sfmData::Observation& observation)
-    {
-        auto costFunction = new ceres::DynamicAutoDiffCostFunction<ProjectionSurveyErrorFunctor>(new ProjectionSurveyErrorFunctor(point, observation, intrinsic));
-
-        int distortionSize = 1;
-        auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
-        if (isod)
-        {
-            auto distortion = isod->getDistortion();
-            if (distortion)
-            {
-                distortionSize = distortion->getParameters().size();
-            }
-        }
-
-        costFunction->AddParameterBlock(intrinsic->getParameters().size());
-        costFunction->AddParameterBlock(distortionSize);
-        costFunction->AddParameterBlock(6);
-        costFunction->SetNumResiduals(2);
-
-        return costFunction;
-    }
-
-    ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor;
-    Vec3 _point;
-};
-
 struct ProjectionErrorFunctor
 {
     explicit ProjectionErrorFunctor(const sfmData::Observation& obs, const std::shared_ptr<camera::IntrinsicBase>& intrinsics)        
diff --git a/src/aliceVision/sfm/bundle/costfunctions/survey.hpp b/src/aliceVision/sfm/bundle/costfunctions/survey.hpp
@@ -0,0 +1,117 @@
+// 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
+
+
+namespace aliceVision {
+namespace sfm {
+
+
+struct SurveyErrorFunctor
+{
+    explicit SurveyErrorFunctor(const Vec3 & point, 
+                                const sfmData::Observation& obs, 
+                                const Vec2 & residual,
+                                const std::shared_ptr<camera::IntrinsicBase>& intrinsics,
+                                double weight)        
+    : _intrinsicFunctor(new CostIntrinsicsProject(obs, intrinsics)), _point(point), _originalResidual(residual), _weight(weight)
+    {        
+    }
+
+    template<typename T>
+    bool operator()(T const* const* parameters, T* residuals) const
+    {       
+        const T* parameter_intrinsics = parameters[0];
+        const T* parameter_distortion = parameters[1];
+        const T* parameter_pose = parameters[2];
+        
+        //Create input point
+        T parameter_point[3];
+        parameter_point[0] = T(_point.x());
+        parameter_point[1] = T(_point.y());
+        parameter_point[2] = T(_point.z());
+
+        //--
+        // Apply external parameters (Pose)
+        //--
+        const T* cam_R = parameter_pose;
+        const T* cam_t = &parameter_pose[3];
+        
+        T transformedPoint[3];
+        // Rotate the point according the camera rotation
+        ceres::AngleAxisRotatePoint(cam_R, parameter_point, transformedPoint);
+
+        // Apply the camera translation
+        transformedPoint[0] += cam_t[0];
+        transformedPoint[1] += cam_t[1];
+        transformedPoint[2] += cam_t[2];
+
+        const T * innerParameters[3];
+        innerParameters[0] = parameter_intrinsics;
+        innerParameters[1] = parameter_distortion;
+        innerParameters[2] = transformedPoint;
+
+        if (!_intrinsicFunctor(innerParameters, residuals))
+        {
+            return false;
+        }
+
+        T epsilon(1e-10);
+        T rx(std::abs(_originalResidual[0]) * 1.1);
+        T ry(std::abs(_originalResidual[1]) * 1.1);
+
+        residuals[0] = sqrt(_weight) * fmax(T(0), sqrt(residuals[0]*residuals[0] + epsilon) - rx);
+        residuals[1] = sqrt(_weight) * fmax(T(0), sqrt(residuals[1]*residuals[1] + epsilon) - ry);
+
+        return true;
+    }
+
+    /**
+     * @brief Create the appropriate cost functor according the provided input camera intrinsic model
+     * @param[in] intrinsicPtr The intrinsic pointer
+     * @param[in] point the  reference point to compare to
+     * @param[in] residual 2d vector with the original residual for the observation
+     * @param[in] observation The corresponding observation
+     * @param[in] weight the scale for the loss
+     * @return cost functor
+     */
+    inline static ceres::CostFunction* createCostFunction(
+                                            const std::shared_ptr<camera::IntrinsicBase> intrinsic, 
+                                            const Vec3 & point,
+                                            const Vec2 & residual,
+                                            const sfmData::Observation& observation,
+                                            double weight)
+    {
+        auto costFunction = new ceres::DynamicAutoDiffCostFunction<SurveyErrorFunctor>(new SurveyErrorFunctor(point, observation, residual, intrinsic, weight));
+
+        int distortionSize = 1;
+        auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
+        if (isod)
+        {
+            auto distortion = isod->getDistortion();
+            if (distortion)
+            {
+                distortionSize = distortion->getParameters().size();
+            }
+        }
+
+        costFunction->AddParameterBlock(intrinsic->getParameters().size());
+        costFunction->AddParameterBlock(distortionSize);
+        costFunction->AddParameterBlock(6);
+        costFunction->SetNumResiduals(2);
+
+        return costFunction;
+    }
+
+    ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor;
+    Vec3 _point;
+    Vec2 _originalResidual;
+    double _weight;
+};
+
+}  // namespace sfm
+}  // namespace aliceVision
