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

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

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

@@ -121,6 +121,14 @@ public:
     // checks if graph is connected and each edge connects exactly 2 nodes
     virtual bool isValid() const = 0;
 
+    // Calculates an initial pose estimate using the Minimum Spanning Tree (Prim's MST) algorithm.
+    // The result serves as a starting point for further optimization.
+    // Edge weights are calculated as:
+    //     weight = translationNorm + lambda * rotationAngle
+    // The default lambda value (0.485) was empirically chosen based on its impact on optimizer performance,
+    // but can/should be tuned for different datasets.
+    virtual void initializePosesWithMST(double lambda = 0.485) = 0;
+
     // creates an optimizer with user-defined settings and returns a pointer on it
     virtual Ptr<LevMarqBase> createOptimizer(const LevMarq::Settings& settings) = 0;
     // creates an optimizer with default settings and returns a pointer on it

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

@@ -0,0 +1,65 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+
+#ifndef OPENCV_3D_MST_HPP
+#define OPENCV_3D_MST_HPP
+
+#include <vector>
+
+namespace cv
+{
+
+/**
+ * @brief Represents an edge in a graph for Minimum Spanning Tree (MST) computation.
+ *
+ * Each edge connects two nodes (source and target) and has an associated weight.
+ */
+struct CV_EXPORTS_W_SIMPLE MSTEdge
+{
+    CV_PROP_RW int source, target;
+    CV_PROP_RW double weight;
+};
+
+/**
+ * @brief Represents the algorithms available for building a Minimum Spanning Tree (MST).
+ *
+ * More algorithms may be added in the future.
+ */
+enum MSTAlgorithm
+{
+    MST_PRIM = 0,
+    MST_KRUSKAL = 1
+};
+
+/**
+ * @brief Builds a Minimum Spanning Tree (MST) using the specified algorithm (see @ref MSTAlgorithm).
+ *
+ * Supports graphs with negative edge weights. Self-loop edges (edges where source and target are the
+ * same) are ignored. If multiple edges exist between the same pair of nodes, only the one with the
+ * lowest weight is considered. If the graph is disconnected or input is invalid, the function
+ * returns false.
+ *
+ * @note The @p root parameter is ignored for algorithms that do not require a starting node.
+ * @note Additional MST algorithms may be supported in the future via the @p algorithm parameter
+ * (see @ref MSTAlgorithm).
+ *
+ * @param numNodes Number of nodes in the graph (must be greater than 0).
+ * @param inputEdges Input vector of edges representing the graph.
+ * @param[out] resultingEdges Output vector to store the edges of the resulting MST.
+ * @param algorithm Specifies which algorithm to use to compute the MST (see @ref MSTAlgorithm).
+ * @param root Starting node for the MST algorithm (only used for certain algorithms).
+ * @return true if a valid MST was successfully built; false otherwise.
+ * @throws cv::Error (StsBadArg) if an invalid algorithm is specified.
+ */
+CV_EXPORTS_W bool buildMST(
+    int numNodes,
+    const std::vector<MSTEdge>& inputEdges,
+    CV_OUT std::vector<MSTEdge>& resultingEdges,
+    MSTAlgorithm algorithm,
+    int root = 0
+);
+
+} // namespace cv
+
+#endif // include guard

modules/3d/src/mst.cpp

@@ -0,0 +1,163 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+
+#include "precomp.hpp"
+#include <opencv2/3d/mst.hpp>
+#include <queue>
+#include <tuple>
+
+namespace
+{
+
+struct DSU
+{
+    std::vector<int> parent, rank;
+    DSU(int n) : parent(n), rank(n, 0)
+    {
+        for (int i = 0; i < n; ++i)
+            parent[i] = i;
+    }
+
+    int find(int x)
+    {
+        if (parent[x] != x)
+            parent[x] = find(parent[x]);
+        return parent[x];
+    }
+
+    void unite(int x, int y)
+    {
+        int rootX = find(x), rootY = find(y);
+        if (rootX != rootY)
+        {
+            if (rank[rootX] < rank[rootY])
+            {
+                parent[rootX] = rootY;
+            } else if (rank[rootX] > rank[rootY])
+            {
+                parent[rootY] = rootX;
+            } else
+            {
+                parent[rootY] = rootX;
+                ++rank[rootX];
+            }
+        }
+    }
+};
+
+bool weightComparator(const cv::MSTEdge& a, const cv::MSTEdge& b)
+{
+    if (a.weight != b.weight)
+        return a.weight < b.weight;
+    if (a.source != b.source)
+        return a.source < b.source;
+    return a.target < b.target;
+}
+
+bool buildMSTKruskal(int numNodes,
+                    const std::vector<cv::MSTEdge>& edges,
+                    std::vector<cv::MSTEdge>& resultingEdges)
+{
+    std::vector<cv::MSTEdge> sortedEdges = edges;
+    std::sort(sortedEdges.begin(), sortedEdges.end(), weightComparator);
+    DSU dsu(numNodes);
+
+    for (const auto &e : sortedEdges)
+    {
+        int u = e.source, v = e.target;
+        if (u >= numNodes || v >= numNodes)
+            return false;
+        if (u == v)
+            continue;
+        if (dsu.find(u) != dsu.find(v))
+        {
+            resultingEdges.push_back(e);
+            dsu.unite(u, v);
+        }
+    }
+
+    return true;
+}
+
+bool buildMSTPrim(int numNodes,
+                  const std::vector<cv::MSTEdge>& edges,
+                  std::vector<cv::MSTEdge>& resultingEdges,
+                  int root)
+{
+    std::vector<bool> inMST(numNodes, false);
+    std::vector<std::vector<cv::MSTEdge>> adj(numNodes);
+    for (const auto& e : edges)
+    {
+        int u = e.source, v = e.target;
+        if (u >= numNodes || v >= numNodes)
+            return false;
+        if (u == v)
+            continue;
+        adj[u].push_back({u, v, e.weight});
+        adj[v].push_back({v, u, e.weight});
+    }
+
+    using HeapElem = std::tuple<double, int, int>; // (weight, from, to)
+    std::priority_queue<HeapElem, std::vector<HeapElem>, std::greater<HeapElem>> pq;
+
+    inMST[root] = true;
+    for (const auto& e : adj[root])
+    {
+        pq.emplace(e.weight, root, e.target);
+    }
+
+    while (!pq.empty())
+    {
+        auto [w, u, v] = pq.top();
+        pq.pop();
+
+        if (inMST[v])
+            continue;
+
+        inMST[v] = true;
+        resultingEdges.push_back({u, v, w});
+        for (const auto& e : adj[v])
+        {
+            if (!inMST[e.target])
+                pq.emplace(e.weight, v, e.target);
+        }
+    }
+
+    return true;
+}
+
+} // unamed namespace
+
+namespace cv
+{
+
+bool buildMST(int numNodes,
+              const std::vector<cv::MSTEdge>& inputEdges,
+              std::vector<cv::MSTEdge>& resultingEdges,
+              MSTAlgorithm algorithm,
+              int root)
+{
+    CV_TRACE_FUNCTION();
+
+    resultingEdges.clear();
+    if (numNodes <= 0 || inputEdges.empty() || root < 0 || root >= numNodes)
+        return false;
+
+    bool result = false;
+    switch (algorithm)
+    {
+        case MST_PRIM:
+            result = buildMSTPrim(numNodes, inputEdges, resultingEdges, root);
+            break;
+        case MST_KRUSKAL:
+            result = buildMSTKruskal(numNodes, inputEdges, resultingEdges);
+            break;
+        default:
+            CV_Error(cv::Error::Code::StsBadArg, "Invalid MST algorithm specified");
+    }
+
+    return (result && resultingEdges.size() == static_cast<size_t>(numNodes - 1));
+}
+
+} // namespace cv

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

@@ -3,8 +3,9 @@
 // of this distribution and at http://opencv.org/license.html
 
 #include "../precomp.hpp"
-#include "sparse_block_matrix.hpp"
 #include "opencv2/3d/detail/optimizer.hpp"
+#include <opencv2/3d/mst.hpp>
+#include "sparse_block_matrix.hpp"
 
 namespace cv
 {
@@ -331,6 +332,8 @@ public:
     // calculate cost function based on provided nodes parameters
     double calcEnergyNodes(const std::map<size_t, Node>& newNodes) const;
 
+    void initializePosesWithMST(double lambda) CV_OVERRIDE;
+
     // creates an optimizer
     virtual Ptr<LevMarqBase> createOptimizer(const LevMarq::Settings& settings) CV_OVERRIDE
     {
@@ -358,6 +361,10 @@ public:
     std::vector<Edge> edges;
 
     Ptr<PoseGraphLevMarq> lm;
+
+private:
+    double calculateWeight(const PoseGraphImpl::Edge& e, double lambda) const;
+    void applyMST(const std::vector<cv::MSTEdge>& resultingEdges, const PoseGraphImpl::Node& rootNode);
 };
 
 
@@ -472,6 +479,110 @@ bool PoseGraphImpl::isValid() const
     return isGraphConnected && !invalidEdgeNode;
 }
 
+double PoseGraphImpl::calculateWeight(const PoseGraphImpl::Edge& e, double lambda) const
+{
+    double translationNorm = cv::norm(e.pose.t);
+
+    cv::Matx33d R = e.pose.q.toRotMat3x3(cv::QUAT_ASSUME_UNIT);
+    cv::Vec3d rvec;
+    cv::Rodrigues(R, rvec);
+    double rotationAngle = cv::norm(rvec);
+
+    double weight = translationNorm + lambda * rotationAngle;
+
+    return weight;
+}
+
+void PoseGraphImpl::applyMST(const std::vector<cv::MSTEdge>& resultingEdges, const PoseGraphImpl::Node& rootNode)
+{
+    std::unordered_map<size_t, std::vector<std::pair<size_t, PoseGraphImpl::Pose3d>>> adj;
+    for (const auto& e: resultingEdges)
+    {
+        auto it = std::find_if(edges.begin(), edges.end(), [&](const PoseGraphImpl::Edge& edge)
+        {
+            return (edge.sourceNodeId == static_cast<size_t>(e.source) && edge.targetNodeId == static_cast<size_t>(e.target)) ||
+                   (edge.sourceNodeId == static_cast<size_t>(e.target) && edge.targetNodeId == static_cast<size_t>(e.source));
+        });
+        if (it != edges.end())
+        {
+            size_t src = it->sourceNodeId;
+            size_t tgt = it->targetNodeId;
+            const PoseGraphImpl::Pose3d& relPose = it->pose;
+
+            adj[src].emplace_back(tgt, relPose);
+            adj[tgt].emplace_back(src, relPose.inverse());
+        }
+    }
+
+    std::unordered_map<size_t, PoseGraphImpl::Pose3d> newPoses;
+    std::stack<size_t> toVisit;
+    std::unordered_set<size_t> visited;
+
+    newPoses[rootNode.id] = rootNode.pose;
+    toVisit.push(rootNode.id);
+
+    while (!toVisit.empty())
+    {
+        size_t current = toVisit.top();
+        toVisit.pop();
+        visited.insert(current);
+
+        const auto& currentPose = newPoses[current];
+
+        auto it = adj.find(current);
+        if (it == adj.end())
+            continue;
+
+        for (const auto& [neighbor, relativePose] : it->second)
+        {
+            if (visited.count(neighbor))
+                continue;
+            newPoses[neighbor] = currentPose * relativePose;
+            toVisit.push(neighbor);
+        }
+    }
+
+    // Apply the new poses
+    for (const auto& [nodeId, pose] : newPoses)
+    {
+        if (!nodes.at(nodeId).isFixed)
+            nodes.at(nodeId).setPose(pose.getAffine());
+    }
+}
+
+void PoseGraphImpl::initializePosesWithMST(double lambda)
+{
+    size_t numNodes = getNumNodes();
+
+    std::vector<MSTEdge> MSTedges;
+    for (const auto& e: edges)
+    {
+        double weight = calculateWeight(e, lambda);
+        MSTedges.push_back({static_cast<int>(e.sourceNodeId), static_cast<int>(e.targetNodeId), weight});
+    }
+
+    size_t rootId = 0;
+    PoseGraphImpl::Node rootNode = nodes.begin()->second;
+
+    for (const auto& n: nodes)
+    {
+        if (isNodeFixed(n.second.id))
+        {
+            rootNode = n.second;
+            rootId = n.second.id;
+            break;
+        }
+    }
+
+    std::vector<MSTEdge> resultingEdges;
+    if (!cv::buildMST(numNodes, MSTedges, resultingEdges, MST_PRIM, static_cast<int>(rootId)))
+    {
+        CV_LOG_INFO(NULL, "Failed to build MST: graph may be disconnected.");
+        return;
+    }
+
+    applyMST(resultingEdges, rootNode);
+}
 
 //////////////////////////
 // Optimization itself //

modules/3d/test/test_mst.cpp

@@ -0,0 +1,402 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+
+#include "test_precomp.hpp"
+#include <opencv2/3d/mst.hpp>
+
+namespace opencv_test {
+namespace {
+
+using namespace cv;
+
+typedef tuple<MSTAlgorithm /*MSTalgorithm*/,
+              int /*numNodes*/,
+              std::vector<MSTEdge>/*edges*/,
+              std::vector<MSTEdge>/*expectedEdges*/
+             > MSTParamType;
+typedef testing::TestWithParam<MSTParamType> MST;
+
+TEST_P(MST, checkCorrectness)
+{
+    const int algorithm = get<0>(GetParam());
+    const int numNodes = get<1>(GetParam());
+    const std::vector<MSTEdge>& edges = get<2>(GetParam());
+    const std::vector<MSTEdge>& expectedEdges = get<3>(GetParam());
+
+    std::vector<MSTEdge> mstEdges;
+    bool result = false;
+
+    switch (algorithm) {
+        case MST_PRIM:
+            // Select first node for root
+            result = buildMST(numNodes, edges, mstEdges, MST_PRIM, 0);
+            break;
+
+        case MST_KRUSKAL:
+            result = buildMST(numNodes, edges, mstEdges, MST_KRUSKAL, 0);
+            break;
+
+        default:
+            FAIL() << "Unknown selected MST algorithm: " << algorithm;
+    }
+
+    EXPECT_TRUE(result);
+    EXPECT_EQ(mstEdges.size(), expectedEdges.size());
+    for (const auto& edge : expectedEdges)
+    {
+        auto it = std::find_if(mstEdges.begin(), mstEdges.end(), [&edge](const MSTEdge& e) {
+            return (e.source == edge.source && e.target == edge.target) ||
+                    (e.source == edge.target && e.target == edge.source);
+        });
+        EXPECT_TRUE(it != mstEdges.end()) << "Missing expected edge: "
+            << edge.source << " -> " << edge.target;
+    }
+}
+
+const MSTParamType mst_graphs[] =
+{
+    // Small Graph
+    MSTParamType(MST_PRIM, 4,
+        {
+            {0, 1, 1.0}, {0, 2, 2.0}, {1, 2, 1.5}, {1, 3, 2.5}, {2, 3, 1.0}
+        },
+        {
+            {0, 1, 1.0}, {1, 2, 1.5}, {2, 3, 1.0}
+        }
+    ),
+
+    MSTParamType(MST_KRUSKAL, 4,
+        {
+            {0, 1, 1.0}, {0, 2, 2.0}, {1, 2, 1.5}, {1, 3, 2.5}, {2, 3, 1.0}
+        },
+        {
+            {0, 1, 1.0}, {1, 2, 1.5}, {2, 3, 1.0}
+        }
+    ),
+
+    // 2 Nodes, 1 Edge
+    MSTParamType(MST_PRIM, 2,
+        {
+            {0, 1, 42.0}
+        },
+        {
+            {0, 1, 42.0}
+        }
+    ),
+
+    MSTParamType(MST_KRUSKAL, 2,
+        {
+            {0, 1, 42.0}
+        },
+        {
+            {0, 1, 42.0}
+        }
+    ),
+
+    // Dense graph (clique)
+    MSTParamType(MST_PRIM, 4,
+        {
+            {0, 1, 1.0}, {0, 2, 2.0}, {0, 3, 3.0},
+            {1, 2, 1.5}, {1, 3, 2.5}, {2, 3, 1.0}
+        },
+        {
+            {0, 1, 1.0}, {2, 3, 1.0}, {1, 2, 1.5}
+        }
+    ),
+
+    MSTParamType(MST_KRUSKAL, 4,
+        {
+            {0, 1, 1.0}, {0, 2, 2.0}, {0, 3, 3.0},
+            {1, 2, 1.5}, {1, 3, 2.5}, {2, 3, 1.0}
+        },
+        {
+            {0, 1, 1.0}, {2, 3, 1.0}, {1, 2, 1.5}
+        }
+    ),
+
+    // Sparse
+    MSTParamType(MST_PRIM, 4,
+        {
+            {0, 1, 1.0}, {1, 2, 2.0}, {1, 3, 3.0}
+        },
+        {
+            {0, 1, 1.0}, {1, 2, 2.0}, {1, 3, 3.0}
+        }
+    ),
+
+    MSTParamType(MST_KRUSKAL, 4,
+        {
+            {0, 1, 1.0}, {1, 2, 2.0}, {1, 3, 3.0}
+        },
+        {
+            {0, 1, 1.0}, {1, 2, 2.0}, {1, 3, 3.0}
+        }
+    ),
+
+    // Weight Floating point check
+    MSTParamType(MST_PRIM, 3,
+        {
+            {0, 1, 1.000001}, {1, 2, 1.000002}, {0, 2, 1.000003}
+        },
+        {
+            {0, 1, 1.000001}, {1, 2, 1.000002}
+        }
+    ),
+
+    MSTParamType(MST_KRUSKAL, 3,
+        {
+            {0, 1, 1.000001}, {1, 2, 1.000002}, {0, 2, 1.000003}
+        },
+        {
+            {0, 1, 1.000001}, {1, 2, 1.000002}
+        }
+    ),
+
+    // 0 or ~0 weight valuess
+    MSTParamType(MST_PRIM, 3,
+        {
+            {0, 1, 0.0}, {1, 2, 1e-9}, {0, 2, 1.0}
+        },
+        {
+            {0, 1, 0.0}, {1, 2, 1e-9}
+        }
+    ),
+
+    MSTParamType(MST_KRUSKAL, 3,
+        {
+            {0, 1, 0.0}, {1, 2, 1e-9}, {0, 2, 1.0}
+        },
+        {
+            {0, 1, 0.0}, {1, 2, 1e-9}
+        }
+    ),
+
+    // Duplicate edges (picks the one with the smallest weight)
+    MSTParamType(MST_PRIM, 3,
+        {
+            {0, 1, 3.0}, {0, 1, 1.0}, {1, 2, 2.0}
+        },
+        {
+            {0, 1, 1.0}, {1, 2, 2.0}
+        }
+    ),
+
+    MSTParamType(MST_KRUSKAL, 3,
+        {
+            {0, 1, 3.0}, {0, 1, 1.0}, {1, 2, 2.0}
+        },
+        {
+            {0, 1, 1.0}, {1, 2, 2.0}
+        }
+    ),
+
+    // Negative weights
+    MSTParamType(MST_PRIM, 3,
+        {
+            {0, 1, -1.0}, {1, 2, -2.0}, {0, 2, -3.0}
+        },
+        {
+            {1, 2, -2.0}, {0, 2, -3.0}
+        }
+    ),
+
+    MSTParamType(MST_KRUSKAL, 3,
+        {
+            {0, 1, -1.0}, {1, 2, -2.0}, {0, 2, -3.0}
+        },
+        {
+            {0, 2, -3.0}, {1, 2, -2.0}
+        }
+    ),
+};
+
+inline static std::string MST_name_printer(const testing::TestParamInfo<MST::ParamType>& info)
+{
+    std::ostringstream os;
+    const auto& algorithm = get<0>(info.param);
+    const auto& numNodes = get<1>(info.param);
+    const auto& edges = get<2>(info.param);
+    const auto& expectedEdges = get<3>(info.param);
+
+    os << "TestCase_" << info.index << "_";
+    switch (algorithm)
+    {
+    case MST_PRIM: os << "Prim"; break;
+    case MST_KRUSKAL: os << "Kruskal"; break;
+    default: os << "Unknown algorithm"; break;
+    }
+    os << "_Nodes_" << numNodes;
+    os << "_Edges_" << edges.size();
+    os << "_ExpectedEdges_" << expectedEdges.size();
+
+    return os.str();
+}
+
+INSTANTIATE_TEST_CASE_P(/**/, MST, testing::ValuesIn(mst_graphs), MST_name_printer);
+
+TEST(MSTStress, largeGraph)
+{
+    const int numNodes = 100000;
+
+    std::vector<MSTEdge> edges;
+
+    for (int i = 0; i < numNodes - 1; ++i)
+        edges.push_back({i, i + 1, static_cast<double>(i + 1)});
+
+    // Add extra edges for complexity
+    for (int i = 0; i < numNodes - 10; i += 10)
+        edges.push_back({i, i + 10,  static_cast<double>(i)});
+    for (int i = 0; i + 20 < numNodes; i += 5)
+        edges.push_back({i, i + 20, static_cast<double>(i + 1)});
+    for (int i = 0; i + 30 < numNodes; i += 3)
+        edges.push_back({i, i + 30, static_cast<double>(i % 50 + 1)});
+    for (int i = 50; i < numNodes; i += 10)
+        edges.push_back({i, i - 25, static_cast<double>(i % 100 + 2)});
+
+    std::vector<MSTEdge> primMST, kruskalMST;
+    bool resultPrim = buildMST(numNodes, edges, primMST, MST_PRIM, 0);
+    bool resultKruskal = buildMST(numNodes, edges, kruskalMST, MST_KRUSKAL, 0);
+
+    EXPECT_TRUE(resultPrim) << "Prim's algorithm failed for large graph ( "
+        << numNodes << " nodes & " << edges.size() << " edges)";
+    EXPECT_TRUE(resultKruskal) << "Kruskal's algorithm failed for large graph ( "
+        << numNodes << " nodes & " << edges.size() << " edges)";
+    EXPECT_EQ(primMST.size(), static_cast<size_t>(numNodes - 1))
+        << "Prim's MST size incorrect for large graph: expected "
+        << (numNodes - 1) << " edges, got " << primMST.size() << ".";
+    EXPECT_EQ(kruskalMST.size(), static_cast<size_t>(numNodes - 1))
+        << "Kruskal's MST size incorrect for large graph: expected "
+        << (numNodes - 1) << " edges, got " << kruskalMST.size() << ".";
+}
+
+typedef tuple<int /*numNodes*/,
+              std::vector<MSTEdge>/*edges*/
+             > MSTNegativeParamType;
+typedef testing::TestWithParam<MSTNegativeParamType> MSTNegative;
+
+TEST_P(MSTNegative, disconnectedGraphs)
+{
+    const int numNodes = get<0>(GetParam());
+    const std::vector<MSTEdge>& edges = get<1>(GetParam());
+
+    std::vector<MSTEdge> primMST, kruskalMST;
+    bool resultPrim = buildMST(numNodes, edges, primMST, MST_PRIM, 0);
+    bool resultKruskal = buildMST(numNodes, edges, kruskalMST, MST_KRUSKAL, 0);
+
+    EXPECT_FALSE(resultPrim) << "Prim's algorithm should fail for disconnected graphs ( "
+        << numNodes << " nodes & " << edges.size() << " edges)";
+    EXPECT_FALSE(resultKruskal) << "Kruskal's algorithm should fail for disconnected graphs ( "
+        << numNodes << " nodes & " << edges.size() << " edges)";
+    EXPECT_LT(primMST.size(), static_cast<size_t>(numNodes - 1))
+        << "Prim's MST should not contain " << numNodes - 1 << " edges for disconnected graphs";
+    EXPECT_LT(kruskalMST.size(), static_cast<size_t>(numNodes - 1))
+        << "Kruskal's MST should not contain " << numNodes - 1 << " edges for disconnected graphs";
+}
+
+const MSTNegativeParamType mst_negative[] =
+{
+    // Two disconnected subgraphs
+
+    MSTNegativeParamType(5,
+        {
+            // Subgraph 1: 0-1
+            {0, 1, 1.0},
+            // Subgraph 2: 2-3-4
+            {2, 3, 2.0}, {3, 4, 3.0}
+        }
+    ),
+
+    MSTNegativeParamType(8,
+        {
+            // Subgraph 1: 0-1-2-3
+            {0, 1, 1.0}, {1, 2, 2.0}, {2, 3, 3.0},
+            // Subgraph 2: 4-5-6-7
+            {4, 5, 4.0}, {5, 6, 5.0}, {6, 7, 6.0}
+        }
+    ),
+
+    MSTNegativeParamType(4,
+        {
+            // Subgraph 1: 0-1
+            {0, 1, 1.0},
+            // Subgraph 2: 2-3
+            {2, 3, 1.0},
+            // self-loop edges (should be ignored by the algorithms)
+            {0, 0, 1.0}, {1, 1, 1.0}, {2, 2, 1.0}, {3, 3, 1.0}
+        }
+    ),
+
+    // Three disconnected components
+
+    MSTNegativeParamType(6,
+        {
+            // Subgraph 1: 0-1
+            {0, 1, 1.0},
+            // Subgraph 2: 2-3
+            {2, 3, 1.0},
+            // Subgraph 3: 4-5
+            {4, 5, 1.0}
+        }
+    ),
+
+    MSTNegativeParamType(9,
+        {
+            // Subgraph 1: 0-1-2-3
+            {0, 1, 1.0}, {1, 2, 1.0}, {2, 3, 1.0},
+            // Subgraph 2: 4-5-6
+            {4, 5, 1.0}, {5, 6, 1.0},
+            // Subgraph 3: 7-8
+            {7, 8, 1.0}
+        }
+    ),
+
+    MSTNegativeParamType(8,
+        {
+            // Subgraph 1: 0-1-2
+            {0, 1, 1.0}, {1, 2, 1.0},
+            // Subgraph 2: 3-4-5
+            {3, 4, 1.0}, {4, 5, 1.0},
+            // Subgraph 3: 6-7
+            {6, 7, 1.0}
+        }
+    ),
+
+    // Isolated nodes
+    MSTNegativeParamType(4,
+        {
+            // Subgraph 1: 0-1-2
+            {0, 1, 1.0}, {1, 2, 1.0},
+            // Isolated node: 3
+        }
+    ),
+
+    MSTNegativeParamType(5,
+        {
+            // Subgraph 1: 0-1-2
+            {0, 1, 1.0}, {1, 2, 1.0},
+            // Isolated nodes: 3, 4
+        }
+    ),
+
+    MSTNegativeParamType(3,
+        {
+            // Isolated node: 0, 1, 2 (no edges)
+        }
+    ),
+};
+
+inline static std::string MST_negative_name_printer(const testing::TestParamInfo<MSTNegative::ParamType>& info) {
+    std::ostringstream os;
+    const auto& numNodes = get<0>(info.param);
+    const auto& edges = get<1>(info.param);
+
+    os << "TestCase_" << info.index << "_Nodes_" << numNodes;
+    os << "_Edges_" << edges.size();
+
+    return os.str();
+}
+
+INSTANTIATE_TEST_CASE_P(/**/, MSTNegative, testing::ValuesIn(mst_negative), MST_negative_name_printer);
+
+}} // namespace

modules/3d/test/test_pose_graph.cpp

@@ -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");

modules/python/src2/cv2.cpp

@@ -46,6 +46,7 @@ typedef std::vector<Scalar> vector_Scalar;
 #ifdef HAVE_OPENCV_OBJDETECT
 typedef std::vector<aruco::Dictionary> vector_Dictionary;
 #endif // HAVE_OPENCV_OBJDETECT
+typedef std::vector<MSTEdge> vector_MSTEdge;
 
 typedef std::vector<std::vector<char> > vector_vector_char;
 typedef std::vector<std::vector<Point> > vector_vector_Point;