Feat #25150: Pose Graph MST initialization #27423

03kiko · 2025-06-09T22:32:57Z

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 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 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

I agree to contribute to the project under Apache 2 License.
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
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.
The feature is well documented and sample code can be built with the project CMake

03kiko · 2025-06-18T15:45:23Z

Hi @asmorkalov,

We noticed that no reviewers have been assigned to this PR yet. Is there a specific reason for this? Let us know if there's anything we can do to move this PR forward.

Thanks!

modules/3d/include/opencv2/3d/detail/mst.hpp

03kiko · 2025-06-22T22:59:01Z

Hi @asmorkalov, thanks for the feedback! Please let us know if everything looks good now or if there’s anything else you'd like us to adjust!

03kiko · 2025-06-26T18:17:42Z

Hi @asmorkalov, could you trigger CI again? It seems to have failed to clone opencv_extra?

03kiko · 2025-07-04T00:43:51Z

@asmorkalov Friendly reminder

modules/3d/include/opencv2/3d/mst.hpp

modules/3d/src/mst.cpp

Implements a MST-based initialization for pose graphs, as proposed in issue opencv#25150. Both Prim’s and Kruskal’s algorithms were added to the 3D module, receiving a vector of node IDs and a vector of edges (each with source/target IDs and a weight) and returning 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 initialization method using MST was implemented. It constructs the MST over the pose graph, then traverses it to reconstruct node poses. A set of test cases validating the implementation was also included. Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

03kiko · 2025-07-11T01:49:30Z

Hi @asmorkalov, @opencv-alalek, we've just pushed a new version addressing your reviews. Please let me know if everything looks good or if there's anything you'd like us to adjust.

03kiko · 2025-07-12T01:21:05Z

Hi @asmorkalov, could you please re-trigger the CI? The Docker pull failed. Thanks!

modules/3d/src/mst.cpp

- Replaces invalid algorithm handling with CV_Error (review comment) - Adds check for disconnected graphs (review comment) - Skips self-loop edges (where source == target) - Adds root < 0 validation - Improves MST Doxygen documentation Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

03kiko · 2025-07-20T02:12:46Z

Hi @asmorkalov, just pushed a new version addressing your comments! Let me know if this looks good to you.

modules/3d/src/rgbd/pose_graph.cpp

modules/3d/test/test_mst.cpp

modules/3d/test/test_pose_graph.cpp

Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

modules/3d/test/test_pose_graph.cpp

03kiko · 2025-07-29T11:26:08Z

We have saveMesh / savePointCloud for obj and ply i/o. Please use it: https://docs.opencv.org/5.x/da/d35/group____3d.html#gad565f2878b3d8c3f959ae7967a520bb2

Looks like this code is not needed any more, as you added implementation with saveMesh below.

Hi @asmorkalov, you're right that part of the code is not needed anymore, and I just left it there for comparison (I will comment out the old code).

Also, please take a look at what I said in my last commit, which you may have missed, and it explains the whole situation better:
Hi, sorry for taking so long to respond.
I've replaced the custom .obj writing logic with a call to cv::saveMesh, as suggested. For reference, the previous version (which I've kept for comparison) explicitly wrote:

v x y z — for each node in the pose graph (as 3D points);
l i j — to represent edges as lines between node indices.
The functions you suggested don’t perfectly fit a pose graph use case:
cv::savePointCloud only captures the nodes — it doesn’t include any edge/connection structure;
cv::saveMesh (used in the current commit) seems to be meant for triangular or polygonal meshes, and thus writes f i j k (faces) instead of l i j (lines) (in the commit we are only passing it 2 vertices, making it write f i j only). As a workaround, we could pass a fake third index by duplicating one vertex (e.g., f i j i to mimic a line), but most 3D viewers recognise these as degenerate faces and don’t render them correctly.

So I’d like to ask for your opinion on the best approach here:

Keep the original version with explicit .obj line writing (v, l);
Continue using cv::saveMesh, feeding 2 or 3 vertex indices (f i j or f i j i), although the pose graphs won't be visible in most 3d viewers;
Use the Mesh struct already implemented in test_pose_graph.cpp (around line ~290);
... or any other direction you’d recommend.

Let me know what you think!

Note: We are using this 3d viewer to open the .obj files and see the pose graph(s).

Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

asmorkalov · 2025-07-29T14:49:55Z

Let's presume the original "hand made" version, but please add a comment why it differs from saveMesh. It's not obvious without detailed code analysis.

Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

03kiko · 2025-08-11T14:29:03Z

Hi @asmorkalov,

Thank you for approving our PR. Could you please merge it when you have a moment? Let us know if there's anything else you need from us.

asmorkalov added feature category: 3d module labels Jun 10, 2025

asmorkalov added this to the 5.0-release milestone Jun 10, 2025

asmorkalov reviewed Jun 20, 2025

View reviewed changes

modules/3d/include/opencv2/3d/detail/mst.hpp Outdated Show resolved Hide resolved

asmorkalov reviewed Jun 20, 2025

View reviewed changes

modules/3d/include/opencv2/3d/detail/mst.hpp Outdated Show resolved Hide resolved

asmorkalov self-assigned this Jun 20, 2025

asmorkalov reviewed Jul 4, 2025

View reviewed changes

modules/3d/include/opencv2/3d/mst.hpp Outdated Show resolved Hide resolved

modules/3d/include/opencv2/3d/mst.hpp Outdated Show resolved Hide resolved

modules/3d/include/opencv2/3d/mst.hpp Outdated Show resolved Hide resolved

opencv-alalek reviewed Jul 4, 2025

View reviewed changes

modules/3d/src/mst.cpp Show resolved Hide resolved

03kiko and others added 4 commits July 10, 2025 12:51

fix: remove trailing whitespaces

bab24a0

fix: changes upon review

e853819

Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

fix: address review comments

7a52f3c

Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

03kiko force-pushed the feat-#25150-MST_init_poseGraph branch from 915069a to 7a52f3c Compare July 10, 2025 12:01

asmorkalov reviewed Jul 15, 2025

View reviewed changes

modules/3d/src/mst.cpp Outdated Show resolved Hide resolved

modules/3d/src/mst.cpp Outdated Show resolved Hide resolved

asmorkalov reviewed Jul 21, 2025

View reviewed changes

modules/3d/src/rgbd/pose_graph.cpp Outdated Show resolved Hide resolved

modules/3d/test/test_mst.cpp Show resolved Hide resolved

modules/3d/test/test_pose_graph.cpp Outdated Show resolved Hide resolved

03kiko and others added 2 commits July 29, 2025 01:07

fix: address review comments

8370e74

Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

fix: removes trailing whitespace(s)

dc0812f

asmorkalov reviewed Jul 29, 2025

View reviewed changes

modules/3d/test/test_pose_graph.cpp Outdated Show resolved Hide resolved

fix: commenting old version

06864de

Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

fix: uses "hand made" version & adds a note on why

fc6705e

Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

fix: variables scope

39d4228

Co-authored-by: Miguel Agra <miguel.agra@tecnico.ulisboa.pt>

asmorkalov approved these changes Jul 31, 2025

View reviewed changes

asmorkalov merged commit 120a46b into opencv:5.x Aug 12, 2025
25 of 26 checks passed

Uh oh!

Feat #25150: Pose Graph MST initialization #27423

Feat #25150: Pose Graph MST initialization #27423

Uh oh!

Conversation

03kiko commented Jun 9, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Notes

Future Work

Pull Request Readiness Checklist

Uh oh!

03kiko commented Jun 18, 2025

Uh oh!

Uh oh!

Uh oh!

03kiko commented Jun 22, 2025

Uh oh!

03kiko commented Jun 26, 2025

Uh oh!

03kiko commented Jul 4, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

03kiko commented Jul 11, 2025

Uh oh!

03kiko commented Jul 12, 2025

Uh oh!

Uh oh!

Uh oh!

03kiko commented Jul 20, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

03kiko commented Jul 29, 2025

Uh oh!

asmorkalov commented Jul 29, 2025

Uh oh!

03kiko commented Aug 11, 2025

Uh oh!

Uh oh!

Uh oh!

03kiko commented Jun 9, 2025 •

edited

Loading