From 066cd6318e51895ef5ba5b9d6c2eb4fbb9050f8f Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Thu, 4 Dec 2025 09:37:33 +0100 Subject: [PATCH 1/2] Add relative landmarks concept --- .../sfm/bundle/BundleAdjustmentCeres.cpp | 27 ++++ .../sfm/bundle/costfunctions/projection.hpp | 124 ++++++++++++++++++ src/aliceVision/sfmData/Landmark.hpp | 8 +- src/aliceVision/sfmDataIO/AlembicExporter.cpp | 4 + src/aliceVision/sfmDataIO/AlembicImporter.cpp | 17 +++ src/aliceVision/sfmDataIO/jsonIO.cpp | 3 +- 6 files changed, 180 insertions(+), 3 deletions(-) diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp index 37e3eff16e..2e11af28d3 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp @@ -537,7 +537,17 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat std::array& 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 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); diff --git a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp index dd4aaa9ba3..e4622f5370 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp +++ b/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& intrinsics + , bool noPose) + : _intrinsicFunctor(new CostIntrinsicsProject(obs, intrinsics)) + , withoutPose(noPose) + { + } + + template + 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 = ¶meter_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 = ¶meter_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 intrinsic, + const sfmData::Observation& observation, + bool withoutPose) + { + auto costFunction = new ceres::DynamicAutoDiffCostFunction(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 diff --git a/src/aliceVision/sfmData/Landmark.hpp b/src/aliceVision/sfmData/Landmark.hpp index 637155bb2b..150e12e2bf 100644 --- a/src/aliceVision/sfmData/Landmark.hpp +++ b/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); } diff --git a/src/aliceVision/sfmDataIO/AlembicExporter.cpp b/src/aliceVision/sfmDataIO/AlembicExporter.cpp index 655d6d67b5..827c4c6dba 100644 --- a/src/aliceVision/sfmDataIO/AlembicExporter.cpp +++ b/src/aliceVision/sfmDataIO/AlembicExporter.cpp @@ -507,6 +507,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks, return; // Fill vector with the values taken from AliceVision + std::vector referenceViewIndices; std::vector positions; std::vector colors; std::vector descTypes; @@ -514,6 +515,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks, 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(landmark.second.descType)); isParallaxRobust.emplace_back(static_cast(landmark.second.isParallaxRobust())); + referenceViewIndices.emplace_back(landmark.second.referenceViewIndex); } std::vector 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) { diff --git a/src/aliceVision/sfmDataIO/AlembicImporter.cpp b/src/aliceVision/sfmDataIO/AlembicImporter.cpp index fdb19af95c..c3d097b2eb 100644 --- a/src/aliceVision/sfmDataIO/AlembicImporter.cpp +++ b/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 diff --git a/src/aliceVision/sfmDataIO/jsonIO.cpp b/src/aliceVision/sfmDataIO/jsonIO.cpp index 17ff429eee..467dc5705b 100644 --- a/src/aliceVision/sfmDataIO/jsonIO.cpp +++ b/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("landmarkId"); landmark.descType = feature::EImageDescriberType_stringToEnum(landmarkTree.get("descType")); landmark.setParallaxRobust(landmarkTree.get("isParallaxRobust", false)); - + landmark.referenceViewIndex = landmarkTree.get("referenceViewIndex", UndefinedIndexT); loadMatrix("color", landmark.rgb, landmarkTree); loadMatrix("X", landmark.X, landmarkTree); From 7ad85cb535c58bf5a187ade14d984501697de2ef Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Fri, 5 Dec 2025 10:03:21 +0100 Subject: [PATCH 2/2] Update bundle adjustment unit test for relative Landmarks --- src/aliceVision/multiview/NViewDataSet.hpp | 2 + .../sfm/bundle/bundleAdjustment_test.cpp | 59 ++++++++++++++++--- 2 files changed, 52 insertions(+), 9 deletions(-) diff --git a/src/aliceVision/multiview/NViewDataSet.hpp b/src/aliceVision/multiview/NViewDataSet.hpp index ebd65d4610..2f24557b5b 100644 --- a/src/aliceVision/multiview/NViewDataSet.hpp +++ b/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; }; /** diff --git a/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp b/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp index ccf4b84b1c..55bc7fc6a0 100644 --- a/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp +++ b/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 ba_object = std::make_shared(); + 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 @@ double RMSE(const SfMData& sfm_data) { // Compute residuals for each observation std::vector 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 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 @@ 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(); + 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; }