Add observed depth constraint in bundle adjustment

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>
@@ -512,125 +513,137 @@
     }
 }
 
 void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem)
 {
     const bool refineStructure = refineOptions & REFINE_STRUCTURE;
 
     // 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();
 
     // build the residual blocks corresponding to the track observations
     for (const auto& landmarkPair : sfmData.getLandmarks())
     {
         const IndexT landmarkId = landmarkPair.first;
         const sfmData::Landmark& landmark = landmarkPair.second;
 
         // do not create a residual block if the landmark
         // have been set as Ignored by the Local BA strategy
         if (landmark.state == EEstimatorParameterState::IGNORED)
         {
             _statistics.addState(EParameter::LANDMARK, EEstimatorParameterState::IGNORED);
             continue;
         }
 
         std::array<double, 3>& landmarkBlock = _landmarksBlocks[landmarkId];
         for (std::size_t i = 0; i < 3; ++i)
             landmarkBlock.at(i) = landmark.X(Eigen::Index(i));
 
         double* landmarkBlockPtr = landmarkBlock.data();
         problem.AddParameterBlock(landmarkBlockPtr, 3);
 
         double* fakeDistortionBlockPtr = &_fakeDistortionBlock;
 
         // add landmark parameter to the all parameters blocks pointers list
         _allParametersBlocks.push_back(landmarkBlockPtr);
 
         // iterate over 2D observation associated to the 3D landmark
         for (const auto& [viewId, observation] : landmark.getObservations())
         {
             const sfmData::View& view = sfmData.getView(viewId);
             const IndexT intrinsicId = view.getIntrinsicId();
 
             if (!sfmData.isPoseAndIntrinsicDefined(view))
             {
                 ALICEVISION_LOG_ERROR("We should not have an undefined pose here");
                 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.getAbsolutePose(view.getPoseId());
             if (pose.getState() == EEstimatorParameterState::IGNORED)
             {
                 ALICEVISION_LOG_ERROR("We should not have an ignored pose here");
                 continue;
             }
 
             // 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];
 
             double * distortionBlockPtr = fakeDistortionBlockPtr;
             if (_distortionsBlocks.find(intrinsicId) != _distortionsBlocks.end())
             {
                 distortionBlockPtr = _distortionsBlocks.at(intrinsicId).data();
             }
 
             // apply a specific parameter ordering:
             if (_ceresOptions.useParametersOrdering)
             {
                 _linearSolverOrdering.AddElementToGroup(landmarkBlockPtr, 0);
                 _linearSolverOrdering.AddElementToGroup(poseBlockPtr, 1);
                 _linearSolverOrdering.AddElementToGroup(intrinsicBlockPtr, 2);
                 _linearSolverOrdering.AddElementToGroup(distortionBlockPtr, 2);
             }
 
             if (view.isPartOfRig() && !view.isPoseIndependant())
             {
                 ceres::CostFunction* costFunction = ProjectionErrorFunctor::createCostFunction(intrinsic, observation);
 
                 double* rigBlockPtr = _rigBlocks.at(view.getRigId()).at(view.getSubPoseId()).data();
                 _linearSolverOrdering.AddElementToGroup(rigBlockPtr, 1);
 
                 std::vector<double*> params;
                 params.push_back(intrinsicBlockPtr);
                 params.push_back(distortionBlockPtr);
                 params.push_back(poseBlockPtr);
                 params.push_back(rigBlockPtr);
                 params.push_back(landmarkBlockPtr);
 
                 problem.AddResidualBlock(costFunction, lossFunction, params);
             }
             else
             {
                 ceres::CostFunction* costFunction = ProjectionSimpleErrorFunctor::createCostFunction(intrinsic, observation);
 
                 std::vector<double*> params;
                 params.push_back(intrinsicBlockPtr);
                 params.push_back(distortionBlockPtr);
                 params.push_back(poseBlockPtr);
                 params.push_back(landmarkBlockPtr);
 
                 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.
                 _statistics.addState(EParameter::LANDMARK, EEstimatorParameterState::CONSTANT);
                 problem.SetParameterBlockConstant(landmarkBlockPtr);
             }
             else
             {
                 _statistics.addState(EParameter::LANDMARK, EEstimatorParameterState::REFINED);
             }
         }
     }
 }
 
 void BundleAdjustmentCeres::addSurveyPointsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem)
 {
 

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

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

@@ -263,6 +263,7 @@ bool SfmTriangulation::processTrackWithPrior(
             o.setFeatureId(trackItem.featureId);
             o.setScale(trackItem.scale);
             o.setCoordinates(trackItem.coords);
+            o.setDepth(trackItem.depth);
         }
 
         //Store the landmark if it's better than the previously estimated one

src/aliceVision/sfmData/Observation.hpp

@@ -31,36 +31,97 @@ class Observation
         _scale(scale_)
     {}
 
+    /**
+    * @brief Comparison operator
+    * @return true if both featureId and coordinates are equal in both observations
+    */
     bool operator==(const Observation& other) const;
 
+    /**
+    * @brief Return the coordinates of the observation in pixels
+    * @return The coordinates in pixels
+    */
     const Vec2& getCoordinates() const { return _coordinates; }
 
+    /**
+    * @brief Return the coordinates of the observation in pixels
+    * @return The coordinates in pixels
+    */
     Vec2& getCoordinates() { return _coordinates; }
 
+    /**
+    * @brief Return the X coordinate of the observation in pixels
+    * @return The column coordinates in pixels
+    */
     double getX() const { return _coordinates.x(); }
 
+    /**
+    * @brief Return the Y coordinate of the observation in pixels
+    * @return The row coordinates in pixels
+    */
     double getY() const { return _coordinates.y(); }
 
+    /**
+    * @brief Set the image coordinates of the observation
+    * @param coordinates the 2d vector with pixel coordinates
+    */
     void setCoordinates(const Vec2& coordinates) { _coordinates = coordinates; }
 
+    /**
+    * @brief Set the image coordinates of the observation
+    * @param x the x coordinates in the image (column)
+    * @param y the x coordinates in the image (row)
+    */
     void setCoordinates(double x, double y)
     {
         _coordinates(0) = x;
         _coordinates(1) = y;
     }
 
+    /**
+    * @brief An observation should be associated to a feature. This is the id of the feature.
+    * @return featureId an id of the feature (view specific)
+    */
     IndexT getFeatureId() const { return _idFeature; }
 
+    /**
+    * @brief An observation should be associated to a feature. This is the id of the feature.
+    * @param featureId an id of the feature (view specific)
+    */
     void setFeatureId(IndexT featureId) { _idFeature = featureId; }
 
+    /**
+    * @brief Get the measured scale of the feature (Scale of the source image)
+    * @return the scale by which the image has been zoomed or 0 if unknown
+    */
     double getScale() const { return _scale; }
 
+    /**
+    * @brief Set the measured scale of the feature (Scale of the source image)
+    * @param scale the scale by which the image has been zoomed or 0 if unknown
+    */
     void setScale(double scale) { _scale = scale; }
 
+    /**
+    * @brief Get the measured depth (Depth meaning is camera type dependent)
+    * @return The optional measured depth, or less than 0 if non used
+    */
+    double getDepth() const { return _depth; }
+
+    /**
+    * @brief Set the measured point depth (Depth meaning is camera type dependent)
+    * @param depth the point depth 
+    */
+    void setDepth(double depth) { _depth = depth; }
+
+
   private:
     Vec2 _coordinates = { 0.0, 0.0 };
     IndexT _idFeature = UndefinedIndexT;
     double _scale = 0.0;
+
+    //Optional measured 'depth'
+    double _depth = -1.0;
 };
 
 /// Observations are indexed by their View_id

src/aliceVision/sfmDataIO/AlembicExporter.cpp

@@ -567,11 +567,13 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
 
         std::vector<float> featPos2d;
         std::vector<float> featScale;
+        std::vector<float> featDepth;
         if (withFeatures)
         {
             featPos2d.reserve(nbObservations * 2);
             visibilityFeatId.reserve(nbObservations);
             featScale.reserve(nbObservations);
+            featDepth.reserve(nbObservations);
         }
 
         for (const auto& landmark : landmarks)
@@ -594,6 +596,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
                     featPos2d.emplace_back(obs.getY());
 
                     featScale.emplace_back(obs.getScale());
+                    featDepth.emplace_back(obs.getDepth());
                 }
             }
         }
@@ -606,6 +609,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
             OUInt32ArrayProperty(userProps, "mvg_visibilityFeatId").set(visibilityFeatId);
             OFloatArrayProperty(userProps, "mvg_visibilityFeatPos").set(featPos2d);  // feature position (x,y)
             OFloatArrayProperty(userProps, "mvg_visibilityFeatScale").set(featScale);
+            OFloatArrayProperty(userProps, "mvg_visibilityFeatDepth").set(featDepth);
         }
     }
     if (!landmarksUncertainty.empty())

src/aliceVision/sfmDataIO/AlembicImporter.cpp

@@ -314,6 +314,7 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData::
         AV_UInt32ArraySamplePtr sampleVisibilityFeatId;
         FloatArraySamplePtr sampleVisibilityFeatPos;
         FloatArraySamplePtr sampleVisibilityFeatScale;
+        FloatArraySamplePtr sampleVisibilityFeatDepth;
 
         if (userProps.getPropertyHeader("mvg_visibilityFeatId") && userProps.getPropertyHeader("mvg_visibilityFeatPos") &&
             flags_part & ESfMData::OBSERVATIONS_WITH_FEATURES)
@@ -329,6 +330,12 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData::
                 propVisibilityFeatScale.get(sampleVisibilityFeatScale);
             }
 
+            if (userProps && userProps.getPropertyHeader("mvg_visibilityFeatDepth"))
+            {
+                IFloatArrayProperty propVisibilityFeatDepth(userProps, "mvg_visibilityFeatDepth");
+                propVisibilityFeatDepth.get(sampleVisibilityFeatDepth);
+            }
+
             if (sampleVisibilityViewId.size() != sampleVisibilityFeatId.size() ||
                 2 * sampleVisibilityViewId.size() != sampleVisibilityFeatPos->size())
             {
@@ -382,6 +389,11 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData::
                     {
                         observation.setScale((*sampleVisibilityFeatScale)[obsGlobalIndex]);
                     }
+
+                    if (sampleVisibilityFeatDepth)
+                    {
+                        observation.setDepth((*sampleVisibilityFeatDepth)[obsGlobalIndex]);
+                    }
                 }
                 else
                 {

src/aliceVision/sfmDataIO/jsonIO.cpp

@@ -527,6 +527,7 @@ void saveLandmark(const std::string& name,
                 obsTree.put("featureId", observation.getFeatureId());
                 saveMatrix("x", observation.getCoordinates(), obsTree);
                 obsTree.put("scale", observation.getScale());
+                obsTree.put("depth", observation.getDepth());
             }
 
             observationsTree.push_back(std::make_pair("", obsTree));
@@ -561,6 +562,7 @@ void loadLandmark(IndexT& landmarkId, sfmData::Landmark& landmark, bpt::ptree& l
                 observation.setFeatureId(obsTree.get<IndexT>("featureId"));
                 loadMatrix("x", observation.getCoordinates(), obsTree);
                 observation.setScale(obsTree.get<double>("scale", 0.0));
+                observation.setDepth(obsTree.get<double>("depth", -1.0));
             }
 
             landmark.getObservations().emplace(obsTree.get<IndexT>("observationId"), observation);

src/aliceVision/sfmDataIO/sfmDataIO.hpp

@@ -12,7 +12,7 @@
 
 #define ALICEVISION_SFMDATAIO_VERSION_MAJOR 1
 #define ALICEVISION_SFMDATAIO_VERSION_MINOR 2
-#define ALICEVISION_SFMDATAIO_VERSION_REVISION 12
+#define ALICEVISION_SFMDATAIO_VERSION_REVISION 13
 
 // AliceVision version as a string; for example "0.9.0".
 #define ALICEVISION_SFMDATAIO_VERSION_STRING                                                                                                         \