Merge pull request #1839 from alicevision/dev/surveyInjecting

Survey Injection

Author: fabiencastan

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

@@ -60,6 +60,31 @@ ceres::CostFunction* createCostFunctionFromIntrinsics(const std::shared_ptr<Intr
     return costFunction;
 }
 
+ceres::CostFunction* createSurveyPointCostFunction(const std::shared_ptr<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;
+}
+
 /**
  * @brief Create the appropriate cost functor according the provided input rig camera intrinsic model
  * @param[in] intrinsicPtr The intrinsic pointer
@@ -723,6 +748,58 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat
     }
 }
 
+void BundleAdjustmentCeres::addSurveyPointsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem)
+{
+   
+    // build the residual blocks corresponding to the track observations
+    for (const auto& [idView, vspoints] : sfmData.getSurveyPoints())
+    {
+        double* fakeDistortionBlockPtr = _fakeDistortionBlock.data();
+
+        const sfmData::View& view = sfmData.getView(idView);
+        const IndexT intrinsicId = view.getIntrinsicId();
+
+        // 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];
+
+        double * distortionBlockPtr = fakeDistortionBlockPtr;
+        if (_distortionsBlocks.find(intrinsicId) != _distortionsBlocks.end())
+        {
+            distortionBlockPtr = _distortionsBlocks.at(intrinsicId).data();
+        }
+
+        // apply a specific parameter ordering:
+        if (_ceresOptions.useParametersOrdering)
+        {
+            _linearSolverOrdering.AddElementToGroup(poseBlockPtr, 1);
+            _linearSolverOrdering.AddElementToGroup(intrinsicBlockPtr, 2);
+            _linearSolverOrdering.AddElementToGroup(distortionBlockPtr, 2);
+        }
+
+
+        for (const auto & spoint: vspoints)
+        {
+            sfmData::Observation observation(spoint.survey, 0, 1.0);
+            ceres::CostFunction* costFunction = createSurveyPointCostFunction(intrinsic, spoint.point3d, observation);
+
+            
+            std::vector<double*> params;
+            params.push_back(intrinsicBlockPtr);
+            params.push_back(distortionBlockPtr);
+            params.push_back(poseBlockPtr);
+
+            problem.AddResidualBlock(costFunction, nullptr, params);
+        }
+    }
+}
+
 void BundleAdjustmentCeres::addConstraints2DToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem)
 {
     // set a LossFunction to be less penalized by false measurements.
@@ -845,6 +922,9 @@ void BundleAdjustmentCeres::createProblem(const sfmData::SfMData& sfmData, ERefi
     // add SfM landmarks to the Ceres problem
     addLandmarksToProblem(sfmData, refineOptions, problem);
 
+    // add SfM landmarks to the Ceres problem
+    addSurveyPointsToProblem(sfmData, refineOptions, problem);
+
     // add 2D constraints to the Ceres problem
     addConstraints2DToProblem(sfmData, refineOptions, problem);
 

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp

@@ -181,6 +181,14 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback
      */
     void addLandmarksToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem);
 
+    /**
+     * @brief Create a residual block for each survey point according to the Ceres format
+     * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics
+     * @param[in] refineOptions The chosen refine flag
+     * @param[out] problem The Ceres bundle adjustment problem
+     */
+    void addSurveyPointsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem);
+
     /**
      * @brief Create a residual block for each 2D constraints
      * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics

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

@@ -61,6 +61,75 @@ 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;
+    }
+
+    ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor;
+    Vec3 _point;
+};
+
 struct ProjectionErrorFunctor
 {
     explicit ProjectionErrorFunctor(const sfmData::Observation& obs, const std::shared_ptr<camera::IntrinsicBase>& intrinsics)        

src/aliceVision/sfmData/CMakeLists.txt

@@ -12,6 +12,7 @@ set(sfmData_files_headers
   ExposureSetting.hpp
   Observation.hpp
   ConstraintPoint.hpp
+  SurveyPoint.hpp
   Constraint2D.hpp
 )
 

src/aliceVision/sfmData/SfMData.hpp

@@ -12,6 +12,7 @@
 #include <aliceVision/sfmData/Constraint2D.hpp>
 #include <aliceVision/sfmData/ConstraintPoint.hpp>
 #include <aliceVision/sfmData/RotationPrior.hpp>
+#include <aliceVision/sfmData/SurveyPoint.hpp>
 #include <aliceVision/sfmData/View.hpp>
 #include <aliceVision/sfmData/Rig.hpp>
 #include <aliceVision/camera/camera.hpp>
@@ -54,9 +55,14 @@ using Constraints2D = std::vector<Constraint2D>;
 /// Define a collection of constraints
 using ConstraintsPoint = std::map<IndexT, ConstraintPoint>;
 
+/// Define a collection of surveyed points
+using SurveyPoints = std::map<IndexT, std::vector<SurveyPoint>>;
+
 /// Define a collection of rotation priors
 using RotationPriors = std::vector<RotationPrior>;
 
+
+
 /**
  * @brief SfMData container
  * Store structure and camera properties
@@ -148,6 +154,13 @@ class SfMData
     const ConstraintsPoint& getConstraintsPoint() const { return _constraintsPoint; }
     ConstraintsPoint& getConstraintsPoint() { return _constraintsPoint; }
 
+    /**
+     * @brief Get SurveyPoints
+     * @return SurveyPoints
+     */
+    const SurveyPoints & getSurveyPoints() const { return _surveyPoints; }
+    SurveyPoints& getSurveyPoints() { return _surveyPoints; }
+
     /**
      * @brief Get RotationPriors
      * @return RotationPriors
@@ -661,6 +674,8 @@ class SfMData
     ConstraintsPoint _constraintsPoint;
     /// Rotation priors
     RotationPriors _rotationpriors;
+    /// Survey points
+    SurveyPoints _surveyPoints;
 
     /**
      * @brief Get Rig pose of a given camera view

src/aliceVision/sfmData/SfMData.i

@@ -14,6 +14,7 @@
 %include <aliceVision/sfmData/CameraPose.i>
 %include <aliceVision/sfmData/Constraint2D.i>
 %include <aliceVision/sfmData/ConstraintPoint.i>
+%include <aliceVision/sfmData/SurveyPoint.i>
 %include <aliceVision/sfmData/Landmark.i>
 %include <aliceVision/sfmData/Observation.i>
 %include <aliceVision/sfmData/Rig.i>

src/aliceVision/sfmData/SurveyPoint.hpp

@@ -0,0 +1,38 @@
+// 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/numeric/numeric.hpp>
+
+namespace aliceVision {
+namespace sfmData {
+
+/**
+ * @brief SurveyPoint is a surveyed 3D point on an image
+ */
+struct SurveyPoint
+{
+    SurveyPoint() = default;
+
+    SurveyPoint(const Vec3 & paramPoint3d, const Vec2 & paramSurvey)
+      : point3d(paramPoint3d), 
+        survey(paramSurvey)
+    {}
+
+    Vec3 point3d;
+    Vec2 survey;
+
+    bool operator==(const SurveyPoint& other) const
+    {
+        return (point3d == other.point3d) && (survey == other.survey);
+    }
+
+    inline bool operator!=(const SurveyPoint& other) const { return !(*this == other); }
+};
+
+}  // namespace sfmData
+}  // namespace aliceVision

src/aliceVision/sfmData/SurveyPoint.i

@@ -0,0 +1,11 @@
+// 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/.
+
+%include <aliceVision/sfmData/SurveyPoint.hpp>
+
+%{
+#include <aliceVision/sfmData/SurveyPoint.hpp>
+%}

src/aliceVision/sfmDataIO/AlembicExporter.cpp

@@ -36,6 +36,7 @@ struct AlembicExporter::DataImpl
         _mvgCloud = Alembic::AbcGeom::OXform(_mvgRoot, "mvgCloud");
         _mvgPointCloud = Alembic::AbcGeom::OXform(_mvgCloud, "mvgPointCloud");
         _mvgAncestors = Alembic::AbcGeom::OXform(_mvgRoot, "mvgAncestors");
+        _mvgSurveys = Alembic::AbcGeom::OXform(_mvgCloud, "mvgSurveys");
 
         // add version as custom property
         const std::vector<::uint32_t> abcVersion = {
@@ -76,6 +77,7 @@ struct AlembicExporter::DataImpl
     Alembic::AbcGeom::OXform _mvgAncestors;
     Alembic::AbcGeom::OXform _mvgCloud;
     Alembic::AbcGeom::OXform _mvgPointCloud;
+    Alembic::AbcGeom::OXform _mvgSurveys;
     Alembic::AbcGeom::OXform _xform;
     Alembic::AbcGeom::OCamera _camObj;
     Alembic::AbcGeom::OUInt32ArrayProperty _propSensorSize_pix;
@@ -358,6 +360,11 @@ void AlembicExporter::addSfM(const sfmData::SfMData& sfmData, ESfMData flagsPart
                      (flagsPart & ESfMData::OBSERVATIONS_WITH_FEATURES));
     }
 
+    if (flagsPart & ESfMData::SURVEYS)
+    {
+        addSurveys(sfmData.getSurveyPoints());
+    }
+
     if (flagsPart & ESfMData::VIEWS || flagsPart & ESfMData::EXTRINSICS)
     {
         std::map<IndexT, std::map<IndexT, std::vector<IndexT>>> rigsViewIds;  // map<rigId,map<poseId,viewId>>
@@ -612,6 +619,32 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
     }
 }
 
+void AlembicExporter::addSurveys(const sfmData::SurveyPoints & points)
+{
+    OPoints outPoints(_dataImpl->_mvgSurveys, "surveys");
+    OPointsSchema& pSchema = outPoints.getSchema();
+    OCompoundProperty userProps = pSchema.getUserProperties();
+
+    std::vector<IndexT> indices;
+    std::vector<double> data;
+
+    for (const auto & [viewId, vsp] : points)
+    {
+        for (const auto & sp : vsp)
+        {
+            indices.push_back(viewId);
+            data.push_back(sp.point3d.x());
+            data.push_back(sp.point3d.y());
+            data.push_back(sp.point3d.z());
+            data.push_back(sp.survey.x());
+            data.push_back(sp.survey.y());
+        }
+    }
+
+    OUInt32ArrayProperty(userProps, "mvg_surveyIds").set(indices);
+    ODoubleArrayProperty(userProps, "mvg_surveyData").set(data);
+}
+
 void AlembicExporter::addCamera(const std::string& name,
                                 const sfmData::View& view,
                                 const sfmData::CameraPose* pose,

src/aliceVision/sfmDataIO/AlembicExporter.hpp

@@ -63,6 +63,12 @@ class AlembicExporter
                       bool withVisibility = true,
                       bool withFeatures = true);
 
+    /**
+     * @brief Add a set of point survey
+     * @param[in] points The survey points to add
+     */
+    void addSurveys(const sfmData::SurveyPoints & points);
+
     /**
      * @brief Add a camera
      * @param[in] name The camera identifier