Merge pull request #2011 from alicevision/dev/depthConstraint

Add observed depth constraint in bundle adjustment

Author: cbentejac

meshroom/aliceVision/CameraInit.py

@@ -665,7 +665,7 @@ def createViewpointsFile(self, node, additionalViews=()):
                     view['metadata'] = json.loads(view['metadata'])
 
             sfmData = {
-                "version": [1, 2, 12],
+                "version": [1, 2, 13],
                 "views": views + newViews,
                 "intrinsics": intrinsics,
                 "featureFolder": "",

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

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                                                                                                         \