Survey cost function

Author: Fabien Servant

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)