add depth constraint to bundle

Author: Fabien Servant

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

@@ -13,6 +13,7 @@
 #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/costfunctions/depth.hpp>
 #include <aliceVision/sfm/bundle/manifolds/intrinsics.hpp>
 #include <aliceVision/sfmData/SfMData.hpp>
 #include <aliceVision/camera/camera.hpp>
@@ -617,6 +618,18 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat
                 problem.AddResidualBlock(costFunction, lossFunction, params);
             }
 
+            if (observation.getDepth() > 0.0)
+            {
+                const double depthVariance = 0.1; //10 cm ?
+                ceres::CostFunction* costFunction = DepthErrorFunctor::createCostFunction(observation, depthVariance);
+ 
+                std::vector<double*> params;
+                params.push_back(poseBlockPtr);
+                params.push_back(landmarkBlockPtr);
+
+                problem.AddResidualBlock(costFunction, nullptr, params);
+            }
+
             if (!refineStructure || landmark.state == EEstimatorParameterState::CONSTANT)
             {
                 // set the whole landmark parameter block as constant.

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

@@ -0,0 +1,71 @@
+// 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/types.hpp>
+#include <ceres/rotation.h>
+
+namespace aliceVision {
+namespace sfm {
+
+/**
+ * Compare the "measured" depth with the estimated point Z position in camera's space
+*/
+struct DepthErrorFunctor
+{
+    explicit DepthErrorFunctor(double depth, double depthVariance) 
+    : _depth(depth), _depthVariance(depthVariance)      
+    {        
+    }
+
+    template<typename T>
+    bool operator()(T const* const* parameters, T* residuals) const
+    {       
+        const T* parameter_pose = parameters[0];
+        const T* parameter_point = parameters[1];
+
+        //--
+        // 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[2] += cam_t[2];
+
+        residuals[0] = T(1.0 / _depthVariance) * (transformedPoint[2] - T(_depth));
+
+        return true;
+    }
+
+    /**
+     * @brief Create the appropriate cost function
+     * @param[in] observation The corresponding observation
+     * @return cost functor
+     */
+    inline static ceres::CostFunction* createCostFunction(
+        const sfmData::Observation& observation, double depthVariance)
+    {
+        auto costFunction = new ceres::DynamicAutoDiffCostFunction<DepthErrorFunctor>(new DepthErrorFunctor(observation.getDepth(), depthVariance));
+
+        costFunction->AddParameterBlock(6);
+        costFunction->AddParameterBlock(3);
+        costFunction->SetNumResiduals(1);
+
+        return costFunction;
+    }
+    
+    const double _depth;
+    const double _depthVariance;
+};
+
+}  // namespace sfm
+}  // namespace aliceVision