potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-30 15:53:03 +04:00
Code Packages Releases Wiki Activity

Merge pull request #27423 from miguel1099:feat-#25150-MST_init_poseGraph

Browse Source 
Feat #25150: Pose Graph MST initialization #27423

Implements an MST-based initialisation for pose graphs, as proposed in issue #25150. Both Prim’s and Kruskal’s algorithms were added to the 3D module. These receive a vector of node IDs and a vector of edges (each with source and target IDs and a weight), and return a vector with the resulting edges. These MST implementations treat edges as undirected internally, meaning users only need to provide one direction (A→B or B→A), and duplicates are handled automatically.

Additionally, a new pose graph initialisation method using MST (Prim) was implemented. It constructs the MST over the pose graph, then traverses it to reconstruct node poses. With this, users can call `poseGraph->initializePosesWithMST()` to create an initial solution for the pose graph problem.

A set of test cases validating the implementation was also included.

#### Notes
- The edge weight used in the MST for pose graphs is calculated as:
`weight = || translation || + λ * rotation_angle`,
where λ = 0.485 was determined empirically based on optimiser performance;
- Validated on [Sphere-a](https://lucacarlone.mit.edu/datasets/) pose graph, showing similar convergence behaviour with or without MST initialisation;
- Alternative weight formulas, such as the Mahalanobis distance formula, were also tested, but the current formula yielded better results.

#### Future Work
- Extend testing to more diverse pose graphs (currently limited to [Sphere-a](https://github.com/opencv/opencv_extra/blob/5.x/testdata/cv/rgbd/sphere_bignoise_vertex3.g2o) in opencv_extra)
- Explore adaptive tuning of the λ parameter for broader applicability.

Co-authored-by: @miguel1099 

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [ ] There is a reference to the original bug report and related work
- [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [x] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Francisco Mónica
2025-08-12 06:22:02 +01:00
committed by GitHub
parent 46a4f6838e
commit 120a46b0d1
7 changed files with 869 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/3d/test/test_pose_graph.cpp
+118
View File
@@ -155,6 +155,119 @@ TEST(PoseGraph, sphereG2O)
}
}
TEST(PoseGraphMST, optimization)
{
applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG);
// The dataset was taken from here: https://lucacarlone.mit.edu/datasets/
// Connected paper:
// L.Carlone, R.Tron, K.Daniilidis, and F.Dellaert.
// Initialization Techniques for 3D SLAM : a Survey on Rotation Estimation and its Use in Pose Graph Optimization.
// In IEEE Intl.Conf.on Robotics and Automation(ICRA), pages 4597 - 4604, 2015.
std::string filename = cvtest::TS::ptr()->get_data_path() + "/cv/rgbd/sphere_bignoise_vertex3.g2o";
Ptr<detail::PoseGraph> pgOptimizerOnly = readG2OFile(filename);
Ptr<detail::PoseGraph> pgWithMSTAndOptimizer = readG2OFile(filename);
Ptr<detail::PoseGraph> init = readG2OFile(filename);
double lambda = 0.485;
pgWithMSTAndOptimizer->initializePosesWithMST(lambda);
// You may change logging level to view detailed optimization report
// For example, set env. variable like this: OPENCV_LOG_LEVEL=INFO
// geoScale=1 is experimental, not guaranteed to work on other problems
// the rest are default params
pgOptimizerOnly->createOptimizer(LevMarq::Settings().setGeoScale(1.0)
.setMaxIterations(100)
.setCheckRelEnergyChange(true)
.setRelEnergyDeltaTolerance(1e-6)
.setGeodesic(true));
pgWithMSTAndOptimizer->createOptimizer(LevMarq::Settings().setGeoScale(1.0)
.setMaxIterations(100)
.setCheckRelEnergyChange(true)
.setRelEnergyDeltaTolerance(1e-6)
.setGeodesic(true));
auto r1 = pgWithMSTAndOptimizer->optimize();
auto r2 = pgOptimizerOnly->optimize();
EXPECT_TRUE(r1.found);
EXPECT_TRUE(r2.found);
EXPECT_LE(r2.energy, 1.47723e+06);
// Allow small tolerance due to optimization differences; final energy/iterations are effectively the same
EXPECT_LE(std::abs(r1.energy - r2.energy), 1e-2);
ASSERT_LE(std::abs(r1.iters - r2.iters), 1);
// Add the "--test_debug" to arguments to see resulting pose graph nodes positions
if (cvtest::debugLevel > 0)
{
// Note:
// A custom .obj writer is used here instead of cv::saveMesh because saveMesh expects faces
// (i.e., polygons with 3 or more vertices) and writes them using the "f i j k..." syntax in
// the .obj file. Since pose graphs consist of edges rather than polygonal faces, we represent
// them using line segments ("l i j").
// As saveMesh does not support writing "l" lines, it is not suitable in this context.
std::string fname;
std::fstream of;
std::vector<size_t> ids;
size_t esz;
// Write OBJ for MST-initialized pose graph with optimizer
fname = "pg_with_mst_and_optimizer.obj";
of.open(fname, std::fstream::out);
ids = pgWithMSTAndOptimizer->getNodesIds();
for (const size_t& id : ids)
{
Point3d d = pgWithMSTAndOptimizer->getNodePose(id).translation();
of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
}
esz = pgWithMSTAndOptimizer->getNumEdges();
for (size_t i = 0; i < esz; i++)
{
size_t sid = pgWithMSTAndOptimizer->getEdgeStart(i), tid = pgWithMSTAndOptimizer->getEdgeEnd(i);
of << "l " << sid + 1 << " " << tid + 1 << std::endl;
}
of.close();
// Write OBJ for optimizer-only pose graph
fname = "pg_optimizer_only.obj";
of.open(fname, std::fstream::out);
ids = pgOptimizerOnly->getNodesIds();
for (const size_t& id : ids)
{
Point3d d = pgOptimizerOnly->getNodePose(id).translation();
of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
}
esz = pgOptimizerOnly->getNumEdges();
for (size_t i = 0; i < esz; i++)
{
size_t sid = pgOptimizerOnly->getEdgeStart(i), tid = pgOptimizerOnly->getEdgeEnd(i);
of << "l " << sid + 1 << " " << tid + 1 << std::endl;
}
of.close();
// Write OBJ for initial pose graph
fname = "pg_init.obj";
of.open(fname, std::fstream::out);
ids = init->getNodesIds();
for (const size_t& id : ids)
{
Point3d d = init->getNodePose(id).translation();
of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
}
esz = init->getNumEdges();
for (size_t i = 0; i < esz; i++)
{
size_t sid = init->getEdgeStart(i), tid = init->getEdgeEnd(i);
of << "l " << sid + 1 << " " << tid + 1 << std::endl;
}
of.close();
}
}
// ------------------------------------------------------------------------------------------
// Wireframe meshes for debugging visualization purposes
@@ -396,6 +509,11 @@ TEST(PoseGraph, sphereG2O)
throw SkipTestException("Build with Eigen required for pose graph optimization");
}
TEST(PoseGraphMST, optimization)
{
throw SkipTestException("Build with Eigen required for pose graph optimization");
}
TEST(PoseGraph, simple)
{
throw SkipTestException("Build with Eigen required for pose graph optimization");