diff --git a/modules/3d/include/opencv2/3d/detail/optimizer.hpp b/modules/3d/include/opencv2/3d/detail/optimizer.hpp index fa480f13cc..57f9af2148 100644 --- a/modules/3d/include/opencv2/3d/detail/optimizer.hpp +++ b/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 createOptimizer(const LevMarq::Settings& settings) = 0; // creates an optimizer with default settings and returns a pointer on it diff --git a/modules/3d/include/opencv2/3d/mst.hpp b/modules/3d/include/opencv2/3d/mst.hpp new file mode 100644 index 0000000000..dad963f677 --- /dev/null +++ b/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 + +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& inputEdges, + CV_OUT std::vector& resultingEdges, + MSTAlgorithm algorithm, + int root = 0 +); + +} // namespace cv + +#endif // include guard diff --git a/modules/3d/src/mst.cpp b/modules/3d/src/mst.cpp new file mode 100644 index 0000000000..e9e67ee146 --- /dev/null +++ b/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 +#include +#include + +namespace +{ + +struct DSU +{ + std::vector 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& edges, + std::vector& resultingEdges) +{ + std::vector 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& edges, + std::vector& resultingEdges, + int root) +{ + std::vector inMST(numNodes, false); + std::vector> 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; // (weight, from, to) + std::priority_queue, std::greater> 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& inputEdges, + std::vector& 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(numNodes - 1)); +} + +} // namespace cv diff --git a/modules/3d/src/rgbd/pose_graph.cpp b/modules/3d/src/rgbd/pose_graph.cpp index 2c0cb77028..ccbc4b74d0 100644 --- a/modules/3d/src/rgbd/pose_graph.cpp +++ b/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 +#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& newNodes) const; + void initializePosesWithMST(double lambda) CV_OVERRIDE; + // creates an optimizer virtual Ptr createOptimizer(const LevMarq::Settings& settings) CV_OVERRIDE { @@ -358,6 +361,10 @@ public: std::vector edges; Ptr lm; + +private: + double calculateWeight(const PoseGraphImpl::Edge& e, double lambda) const; + void applyMST(const std::vector& 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& resultingEdges, const PoseGraphImpl::Node& rootNode) +{ + std::unordered_map>> adj; + for (const auto& e: resultingEdges) + { + auto it = std::find_if(edges.begin(), edges.end(), [&](const PoseGraphImpl::Edge& edge) + { + return (edge.sourceNodeId == static_cast(e.source) && edge.targetNodeId == static_cast(e.target)) || + (edge.sourceNodeId == static_cast(e.target) && edge.targetNodeId == static_cast(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 newPoses; + std::stack toVisit; + std::unordered_set 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 MSTedges; + for (const auto& e: edges) + { + double weight = calculateWeight(e, lambda); + MSTedges.push_back({static_cast(e.sourceNodeId), static_cast(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 resultingEdges; + if (!cv::buildMST(numNodes, MSTedges, resultingEdges, MST_PRIM, static_cast(rootId))) + { + CV_LOG_INFO(NULL, "Failed to build MST: graph may be disconnected."); + return; + } + + applyMST(resultingEdges, rootNode); +} ////////////////////////// // Optimization itself // diff --git a/modules/3d/test/test_mst.cpp b/modules/3d/test/test_mst.cpp new file mode 100644 index 0000000000..8db42b2395 --- /dev/null +++ b/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 + +namespace opencv_test { +namespace { + +using namespace cv; + +typedef tuple/*edges*/, + std::vector/*expectedEdges*/ + > MSTParamType; +typedef testing::TestWithParam MST; + +TEST_P(MST, checkCorrectness) +{ + const int algorithm = get<0>(GetParam()); + const int numNodes = get<1>(GetParam()); + const std::vector& edges = get<2>(GetParam()); + const std::vector& expectedEdges = get<3>(GetParam()); + + std::vector 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& 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 edges; + + for (int i = 0; i < numNodes - 1; ++i) + edges.push_back({i, i + 1, static_cast(i + 1)}); + + // Add extra edges for complexity + for (int i = 0; i < numNodes - 10; i += 10) + edges.push_back({i, i + 10, static_cast(i)}); + for (int i = 0; i + 20 < numNodes; i += 5) + edges.push_back({i, i + 20, static_cast(i + 1)}); + for (int i = 0; i + 30 < numNodes; i += 3) + edges.push_back({i, i + 30, static_cast(i % 50 + 1)}); + for (int i = 50; i < numNodes; i += 10) + edges.push_back({i, i - 25, static_cast(i % 100 + 2)}); + + std::vector 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(numNodes - 1)) + << "Prim's MST size incorrect for large graph: expected " + << (numNodes - 1) << " edges, got " << primMST.size() << "."; + EXPECT_EQ(kruskalMST.size(), static_cast(numNodes - 1)) + << "Kruskal's MST size incorrect for large graph: expected " + << (numNodes - 1) << " edges, got " << kruskalMST.size() << "."; +} + +typedef tuple/*edges*/ + > MSTNegativeParamType; +typedef testing::TestWithParam MSTNegative; + +TEST_P(MSTNegative, disconnectedGraphs) +{ + const int numNodes = get<0>(GetParam()); + const std::vector& edges = get<1>(GetParam()); + + std::vector 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(numNodes - 1)) + << "Prim's MST should not contain " << numNodes - 1 << " edges for disconnected graphs"; + EXPECT_LT(kruskalMST.size(), static_cast(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& 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 diff --git a/modules/3d/test/test_pose_graph.cpp b/modules/3d/test/test_pose_graph.cpp index be1676a8eb..a2f345f108 100644 --- a/modules/3d/test/test_pose_graph.cpp +++ b/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 pgOptimizerOnly = readG2OFile(filename); + Ptr pgWithMSTAndOptimizer = readG2OFile(filename); + Ptr 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 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"); diff --git a/modules/python/src2/cv2.cpp b/modules/python/src2/cv2.cpp index c23cac483d..7cbeff00ee 100644 --- a/modules/python/src2/cv2.cpp +++ b/modules/python/src2/cv2.cpp @@ -46,6 +46,7 @@ typedef std::vector vector_Scalar; #ifdef HAVE_OPENCV_OBJDETECT typedef std::vector vector_Dictionary; #endif // HAVE_OPENCV_OBJDETECT +typedef std::vector vector_MSTEdge; typedef std::vector > vector_vector_char; typedef std::vector > vector_vector_Point;