landmarks can now be defined with respect to a camera geometric frame instead of the world geometric frame

src/aliceVision/multiview/NViewDataSet.hpp

@@ -61,6 +61,8 @@ struct NViewDatasetConfigurator
     double _jitter_amount;
 
     NViewDatasetConfigurator(int fx = 1000, int fy = 1000, int cx = 500, int cy = 500, double distance = 1.5, double jitter_amount = 0.01);
+
+    bool _useRelative = false;
 };
 
 /**

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

@@ -509,114 +509,141 @@
     }
 }
 
 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));
+        }
+
+        //If the landmark has a referenceViewIndex set, then retrieve the reference pose
+        double * referencePoseBlockPtr = nullptr;
+        if (landmark.referenceViewIndex != UndefinedIndexT)
+        {
+            const sfmData::View& refview = sfmData.getView(landmark.referenceViewIndex);
+            referencePoseBlockPtr = _posesBlocks.at(refview.getPoseId()).data();
+        }
 
         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();
 
             // 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(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);
             }
+            if (referencePoseBlockPtr != nullptr)
+            {
+                bool samePose = (referencePoseBlockPtr == poseBlockPtr);
+                ceres::CostFunction* costFunction = ProjectionRelativeErrorFunctor::createCostFunction(intrinsic, observation, samePose);
+
+                std::vector<double*> params;
+                params.push_back(intrinsicBlockPtr);
+                params.push_back(distortionBlockPtr);
+                if (!samePose)
+                {
+                    params.push_back(poseBlockPtr);
+                    params.push_back(referencePoseBlockPtr);
+                }
+                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 (!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/bundleAdjustment_test.cpp

@@ -54,6 +54,27 @@ BOOST_AUTO_TEST_CASE(BUNDLE_ADJUSTMENT_EffectiveMinimization_Pinhole)
     BOOST_CHECK_LT(dResidual_after, dResidual_before);
 }
 
+BOOST_AUTO_TEST_CASE(BUNDLE_ADJUSTMENT_EffectiveMinimization_Relative_Pinhole)
+{
+    const int nviews = 3;
+    const int npoints = 6;
+    NViewDatasetConfigurator config;
+    config._useRelative = true;
+    const NViewDataSet d = NRealisticCamerasRing(nviews, npoints, config);
+
+    // Translate the input dataset to a SfMData scene
+    SfMData sfmData = getInputScene(d, config, EINTRINSIC::PINHOLE_CAMERA, EDISTORTION::DISTORTION_NONE);
+
+    const double dResidual_before = RMSE(sfmData);
+
+    // Call the BA interface and let it refine (Structure and Camera parameters [Intrinsics|Motion])
+    std::shared_ptr<BundleAdjustment> ba_object = std::make_shared<BundleAdjustmentCeres>();
+    BOOST_CHECK(ba_object->adjust(sfmData));
+
+    const double dResidual_after = RMSE(sfmData);
+    BOOST_CHECK_LT(dResidual_after, dResidual_before);
+}
+
 BOOST_AUTO_TEST_CASE(BUNDLE_ADJUSTMENT_EffectiveMinimization_PinholeRadialK1)
 {
     const int nviews = 3;
@@ -308,6 +329,15 @@ SfMData getInputScene(const NViewDataSet& d, const NViewDatasetConfigurator& con
         // Collect the image of point i in each frame.
         Landmark landmark;
         landmark.X = d._X.col(i);
+
+        if (config._useRelative)
+        {
+            landmark.referenceViewIndex = nviews / 2;
+            geometry::Pose3 p = sfm_data.getAbsolutePose(landmark.referenceViewIndex).getTransform();
+            geometry::Pose3 pinv = p.inverse();
+            landmark.X = pinv(landmark.X);
+        }
+
         for (int j = 0; j < nviews; ++j)
         {
             Vec2 pt = d._x[j].col(i);
@@ -317,6 +347,7 @@ SfMData getInputScene(const NViewDataSet& d, const NViewDatasetConfigurator& con
 
             landmark.getObservations()[j] = Observation(pt, i, unknownScale);
         }
+
         sfm_data.getLandmarks()[i] = landmark;
     }
 

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

@@ -281,6 +281,130 @@ struct ProjectionErrorFunctor
     ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor;
 };
 
+struct ProjectionRelativeErrorFunctor
+{
+    explicit ProjectionRelativeErrorFunctor(const sfmData::Observation& obs
+    , const std::shared_ptr<camera::IntrinsicBase>& intrinsics
+    , bool noPose)        
+    : _intrinsicFunctor(new CostIntrinsicsProject(obs, intrinsics))
+    , withoutPose(noPose)
+    {        
+    }
+
+    template<typename T>
+    bool operator()(T const* const* parameters, T* residuals) const
+    {       
+        const T* parameter_intrinsics = parameters[0];
+        const T* parameter_distortion = parameters[1];
+
+
+        T transformedPoint[3];
+        if (withoutPose)
+        {
+            //withoutPose is used if reference view and current view are the same views
+            const T* parameter_relativepoint = parameters[2];
+            transformedPoint[0] = parameter_relativepoint[0];
+            transformedPoint[1] = parameter_relativepoint[1];
+            transformedPoint[2] = parameter_relativepoint[2];
+        }
+        else
+        {
+            const T* parameter_pose = parameters[2];
+            const T* parameter_refpose = parameters[3];
+            const T* parameter_relativepoint = parameters[4];
+
+            //Retrieve point
+            T relpoint[3];
+            relpoint[0] = parameter_relativepoint[0];
+            relpoint[1] = parameter_relativepoint[1];
+            relpoint[2] = parameter_relativepoint[2];
+
+            const T* refcam_R = parameter_refpose;
+            const T* refcam_t = &parameter_refpose[3];
+
+            // Apply transformation such that the point
+            // Which was defined in the camera geometric frame
+            // is now defined in the world frame
+            relpoint[0] = relpoint[0] - refcam_t[0];
+            relpoint[1] = relpoint[1] - refcam_t[1];
+            relpoint[2] = relpoint[2] - refcam_t[2];
+
+            T invrefcam_R[3];
+            invrefcam_R[0] = -refcam_R[0];
+            invrefcam_R[1] = -refcam_R[1];
+            invrefcam_R[2] = -refcam_R[2];
+
+            T absolutePoint[3];  
+            ceres::AngleAxisRotatePoint(invrefcam_R, relpoint, absolutePoint);
+
+            //--
+            // Apply external parameters (Pose)
+            //--
+            const T* cam_R = parameter_pose;
+            const T* cam_t = &parameter_pose[3];
+
+
+            // Rotate the point according the camera rotation
+            ceres::AngleAxisRotatePoint(cam_R, absolutePoint, 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;
+
+        return _intrinsicFunctor(innerParameters, residuals);
+    }
+
+    /**
+     * @brief Create the appropriate cost functor according the provided input camera intrinsic model
+     * @param[in] intrinsicPtr The intrinsic pointer
+     * @param[in] observation The corresponding observation
+     * @param[in] withoutPose When reference view and current view are the same, poses must be ignored
+     * @return cost functor
+     */
+    inline static ceres::CostFunction* createCostFunction(
+        const std::shared_ptr<camera::IntrinsicBase> intrinsic, 
+        const sfmData::Observation& observation,
+        bool withoutPose)
+    {
+        auto costFunction = new ceres::DynamicAutoDiffCostFunction<ProjectionRelativeErrorFunctor>(new ProjectionRelativeErrorFunctor(observation, intrinsic, withoutPose));
+
+        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);
+
+        if (!withoutPose)
+        {
+            costFunction->AddParameterBlock(6);
+            costFunction->AddParameterBlock(6);
+        }
+
+        costFunction->AddParameterBlock(3);
+        costFunction->SetNumResiduals(2);
+
+        return costFunction;
+    }
+
+    ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor;
+    bool withoutPose;
+};
+
 
 }  // namespace sfm
 }  // namespace aliceVision

src/aliceVision/sfmData/Landmark.hpp

@@ -41,11 +41,15 @@ class Landmark
     feature::EImageDescriberType descType = feature::EImageDescriberType::UNINITIALIZED;
     image::RGBColor rgb = image::WHITE;  //!> the color associated to the point
     EEstimatorParameterState state = EEstimatorParameterState::REFINED;
+    IndexT referenceViewIndex = UndefinedIndexT;
 
     bool operator==(const Landmark& other) const
     {
-        return AreVecNearEqual(X, other.X, 1e-3) && AreVecNearEqual(rgb, other.rgb, 1e-3) && _observations == other._observations &&
-               descType == other.descType;
+        return AreVecNearEqual(X, other.X, 1e-3) 
+        && AreVecNearEqual(rgb, other.rgb, 1e-3) 
+        && _observations == other._observations 
+        && descType == other.descType
+        && referenceViewIndex == other.referenceViewIndex;
     }
 
     inline bool operator!=(const Landmark& other) const { return !(*this == other); }

src/aliceVision/sfmDataIO/AlembicExporter.cpp

@@ -505,11 +505,13 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
         return;
 
     // Fill vector with the values taken from AliceVision
+    std::vector<IndexT> referenceViewIndices;
     std::vector<V3f> positions;
     std::vector<Imath::C3f> colors;
     std::vector<Alembic::Util::uint32_t> descTypes;
     positions.reserve(landmarks.size());
     descTypes.reserve(landmarks.size());
+    referenceViewIndices.reserve(landmarks.size());
 
     // For all the 3d points in the hash_map
     for (const auto& landmark : landmarks)
@@ -520,6 +522,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
         positions.emplace_back(pt[0], -pt[1], -pt[2]);
         colors.emplace_back(color.r() / 255.f, color.g() / 255.f, color.b() / 255.f);
         descTypes.emplace_back(static_cast<Alembic::Util::uint8_t>(landmark.second.descType));
+        referenceViewIndices.emplace_back(landmark.second.referenceViewIndex);
     }
 
     std::vector<Alembic::Util::uint64_t> ids(positions.size());
@@ -542,6 +545,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
     OCompoundProperty userProps = pSchema.getUserProperties();
 
     OUInt32ArrayProperty(userProps, "mvg_describerType").set(descTypes);
+    OUInt32ArrayProperty(userProps, "mvg_referenceViewIndices").set(referenceViewIndices);
 
     if (withVisibility)
     {

src/aliceVision/sfmDataIO/AlembicImporter.cpp

@@ -186,6 +186,18 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData::
         }
     }
 
+    AV_UInt32ArraySamplePtr sampleReferenceViewIndices;
+    if (userProps && userProps.getPropertyHeader("mvg_referenceViewIndices"))
+    {
+        sampleReferenceViewIndices.read(userProps, "mvg_referenceViewIndices");
+        if (sampleReferenceViewIndices.size() != positions->size())
+        {
+            ALICEVISION_LOG_WARNING("[Alembic Importer] Describer type will be ignored. describerType vector size: "
+                                    << sampleReferenceViewIndices.size() << ", positions vector size: " << positions->size());
+            sampleReferenceViewIndices.reset();
+        }
+    }
+
     // Number of points before adding the Alembic data
     const std::size_t nbPointsInit = sfmdata.getLandmarks().size();
     for (std::size_t point3d_i = 0; point3d_i < positions->size(); ++point3d_i)
@@ -217,6 +229,11 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData::
             const std::size_t descType_i = sampleDescs[point3d_i];
             landmark.descType = static_cast<feature::EImageDescriberType>(descType_i);
         }
+
+        if (sampleReferenceViewIndices)
+        {
+            landmark.referenceViewIndex = sampleReferenceViewIndices[point3d_i];
+        }
     }
 
     // for compatibility with files generated with a previous version

src/aliceVision/sfmDataIO/jsonIO.cpp

@@ -500,6 +500,7 @@ void saveLandmark(const std::string& name,
     bpt::ptree landmarkTree;
 
     landmarkTree.put("landmarkId", landmarkId);
+    landmarkTree.put("referenceViewIndex", landmark.referenceViewIndex);
     landmarkTree.put("descType", feature::EImageDescriberType_enumToString(landmark.descType));
 
     saveMatrix("color", landmark.rgb, landmarkTree);
@@ -538,7 +539,7 @@ void loadLandmark(IndexT& landmarkId, sfmData::Landmark& landmark, bpt::ptree& l
 {
     landmarkId = landmarkTree.get<IndexT>("landmarkId");
     landmark.descType = feature::EImageDescriberType_stringToEnum(landmarkTree.get<std::string>("descType"));
-
+    landmark.referenceViewIndex = landmarkTree.get<IndexT>("referenceViewIndex", UndefinedIndexT);
     loadMatrix("color", landmark.rgb, landmarkTree);
     loadMatrix("X", landmark.X, landmarkTree);