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
