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

@@ -537,7 +537,17 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat
 
         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);
@@ -605,6 +615,23 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat
 
                 problem.AddResidualBlock(costFunction, lossFunction, params);
             }
+            else 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);

src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp

@@ -54,6 +54,27 @@
     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;
@@ -246,15 +267,26 @@
 {
     // Compute residuals for each observation
     std::vector<double> vec;
-    for (Landmarks::const_iterator iterTracks = sfm_data.getLandmarks().begin(); iterTracks != sfm_data.getLandmarks().end(); ++iterTracks)
+    for (const auto & [lid, landmark]: sfm_data.getLandmarks())
     {
-        const Observations& observations = iterTracks->second.getObservations();
-        for (Observations::const_iterator itObs = observations.begin(); itObs != observations.end(); ++itObs)
+        const Observations& observations = landmark.getObservations();
+
+
+        Pose3 world_T_reference;
+        if (landmark.referenceViewIndex != UndefinedIndexT)
         {
-            const View* view = sfm_data.getViews().find(itObs->first)->second.get();
-            const Pose3 pose = sfm_data.getPose(*view).getTransform();
-            const std::shared_ptr<IntrinsicBase> intrinsic = sfm_data.getIntrinsics().find(view->getIntrinsicId())->second;
-            const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.getCoordinates());
+            const View & refView = sfm_data.getView(landmark.referenceViewIndex);
+            world_T_reference = sfm_data.getPose(refView).getTransform().inverse();
+        }
+
+        for (const auto [idView, observation] : observations)
+        {
+            const View & view = sfm_data.getView(idView);
+            const Pose3 camera_T_world = sfm_data.getPose(view).getTransform();
+            const Pose3 camera_T_reference = camera_T_world * world_T_reference;
+            const IntrinsicBase & intrinsic = sfm_data.getIntrinsic(view.getIntrinsicId());
+            const Vec2 residual = intrinsic.residual(camera_T_reference, landmark.X.homogeneous(), observation.getCoordinates());
+
             vec.push_back(residual(0));
             vec.push_back(residual(1));
         }
@@ -308,15 +340,24 @@
         // 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();
+            landmark.X = p(landmark.X);
+        }
+
         for (int j = 0; j < nviews; ++j)
         {
             Vec2 pt = d._x[j].col(i);
             // => random noise between [-.5,.5] is added
-            pt(0) += rand() / RAND_MAX - .5;
-            pt(1) += rand() / RAND_MAX - .5;
+            pt(0) += double(rand()) / double(RAND_MAX) - .5;
+            pt(1) += double(rand()) / double(RAND_MAX) - .5;
 
             landmark.getObservations()[j] = Observation(pt, i, unknownScale);
         }
+
         sfm_data.getLandmarks()[i] = landmark;
     }
 

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

@@ -282,6 +282,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

@@ -507,13 +507,15 @@ 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;
     std::vector<Alembic::Util::bool_t> isParallaxRobust;
     positions.reserve(landmarks.size());
     descTypes.reserve(landmarks.size());
     isParallaxRobust.reserve(landmarks.size());
+    referenceViewIndices.reserve(landmarks.size());
 
     // For all the 3d points in the hash_map
     for (const auto& landmark : landmarks)
@@ -525,6 +527,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
         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));
         isParallaxRobust.emplace_back(static_cast<Alembic::Util::bool_t>(landmark.second.isParallaxRobust()));
+        referenceViewIndices.emplace_back(landmark.second.referenceViewIndex);
     }
 
     std::vector<Alembic::Util::uint64_t> ids(positions.size());
@@ -548,6 +551,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks,
 
     OUInt32ArrayProperty(userProps, "mvg_describerType").set(descTypes);
     OBoolArrayProperty(userProps, "mvg_isParallaxRobust").set(isParallaxRobust);
+    OUInt32ArrayProperty(userProps, "mvg_referenceViewIndices").set(referenceViewIndices);
 
     if (withVisibility)
     {

src/aliceVision/sfmDataIO/AlembicImporter.cpp

@@ -200,6 +200,18 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData::
             sampleIsParallaxRobust->reset();
         }
     }
+
+    AV_UInt32ArraySamplePtr sampleReferenceViewIndices;
+    if (userProps && userProps.getPropertyHeader("mvg_referenceViewIndices"))
+    {
+        sampleReferenceViewIndices.read(userProps, "mvg_referenceViewIndices");
+        if (sampleReferenceViewIndices.size() != positions->size())
+        {
+            ALICEVISION_LOG_WARNING("[Alembic Importer] Reference views will be ignored. 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();
@@ -238,6 +250,11 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData::
             const bool isParallaxRobust = sampleIsParallaxRobust->get()[point3d_i];
             landmark.setParallaxRobust(isParallaxRobust);
         }
+
+        if (sampleReferenceViewIndices)
+        {
+            landmark.referenceViewIndex = sampleReferenceViewIndices[point3d_i];
+        }
     }
 
     // for compatibility with files generated with a previous version

src/aliceVision/sfmDataIO/jsonIO.cpp

@@ -503,6 +503,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));
     landmarkTree.put("isParallaxRobust", landmark.isParallaxRobust());
 
@@ -544,7 +545,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.setParallaxRobust(landmarkTree.get<bool>("isParallaxRobust", false));
-
+    landmark.referenceViewIndex = landmarkTree.get<IndexT>("referenceViewIndex", UndefinedIndexT);
     loadMatrix("color", landmark.rgb, landmarkTree);
     loadMatrix("X", landmark.X, landmarkTree);