Add connected components

Author: RainerKuemmerle

g2o/core/sparse_optimizer.cpp

@@ -31,6 +31,7 @@
 #include <cassert>
 #include <cstdlib>
 #include <iostream>
+#include <limits>
 #include <memory>
 #include <string_view>
 #include <unordered_set>
@@ -40,6 +41,7 @@
 #include "estimate_propagator.h"
 #include "g2o/config.h"  // IWYU pragma: keep
 #include "g2o/core/eigen_types.h"
+#include "g2o/core/hyper_dijkstra.h"
 #include "g2o/core/jacobian_workspace.h"  // IWYU pragma: keep
 #include "g2o/core/optimizable_graph.h"
 #include "g2o/core/sparse_block_matrix.h"
@@ -164,6 +166,50 @@ void SparseOptimizer::printGraphSummary(std::ostream& os) const {
     os << "  active chi2: " << activeChi2() << "\n";
     os << "  active robust chi2: " << activeRobustChi2() << "\n";
   }
+  os << "  connected_components: " << numConnectedComponents() << "\n";
+}
+
+int SparseOptimizer::numConnectedComponents(int level) const {
+  if (vertices().empty()) return 0;
+
+  struct LevelCostFunction : public HyperDijkstra::CostFunction {
+    explicit LevelCostFunction(int level_) : level(level_) {}
+    double operator()(HyperGraph::Edge* edge, HyperGraph::Vertex* /*from*/,
+                      HyperGraph::Vertex* /*to*/) override {
+      auto oe = static_cast<OptimizableGraph::Edge*>(edge);
+      if (!oe || oe->level() != level) {
+        return std::numeric_limits<double>::max();
+      }
+      return 1.;
+    }
+    int level;
+  };
+
+  std::shared_ptr<HyperGraph> graph(const_cast<SparseOptimizer*>(this),
+                                    [](HyperGraph*) {});
+  HyperDijkstra d(graph);
+  LevelCostFunction cost(level);
+
+  HyperGraph::VertexSet remaining;
+  for (const auto& it : vertices()) {
+    remaining.insert(it.second);
+  }
+
+  int components = 0;
+  while (!remaining.empty()) {
+    auto root = *remaining.begin();
+    d.shortestPaths(root, cost);
+    const auto& visited = d.visited();
+    for (const auto& v : visited) {
+      remaining.erase(v);
+    }
+    components++;
+  }
+  return components;
+}
+
+bool SparseOptimizer::isConnected(int level) const {
+  return numConnectedComponents(level) <= 1;
 }
 
 std::shared_ptr<OptimizableGraph::Vertex> SparseOptimizer::findGauge() {

g2o/core/sparse_optimizer.h

@@ -177,6 +177,18 @@ class G2O_CORE_API SparseOptimizer : public OptimizableGraph {
    */
   void printGraphSummary(std::ostream& os = std::cout) const;
 
+  /**
+   * Returns the number of connected components in the graph.
+   * Only edges with the specified level are considered.
+   */
+  int numConnectedComponents(int level = 0) const;
+
+  /**
+   * Returns true if the graph is fully connected.
+   * Only edges with the specified level are considered.
+   */
+  bool isConnected(int level = 0) const;
+
   //! verbose information during optimization
   bool verbose() const { return verbose_; }
   void setVerbose(bool verbose);

python/core/py_sparse_optimizer.cpp

@@ -84,6 +84,9 @@ void declareSparseOptimizer(py::module& m) {
       .def("gauge_freedom", &CLS::gaugeFreedom)  // -> bool
       .def("active_chi2", &CLS::activeChi2)      // -> double
       .def("active_robust_chi2", &CLS::activeRobustChi2)  // -> double
+      .def("num_connected_components", &CLS::numConnectedComponents,
+           "level"_a = 0)
+      .def("is_connected", &CLS::isConnected, "level"_a = 0)
       .def("print_graph_summary",
            [](CLS& optimizer) {
              std::ostringstream os;

python/tests/test_core.py

@@ -57,6 +57,22 @@ def test_print_graph_summary(self, simple_se2_graph):
         assert "vertices:" in summary
         assert "edges:" in summary
         assert "poses:" in summary
+        assert "levels:" in summary
+        assert "connected_components:" in summary
+
+    def test_connected_components(self, simple_se2_graph):
+        """Test graph connectivity diagnostics."""
+        assert simple_se2_graph.is_connected()
+        assert simple_se2_graph.num_connected_components() == 1
+
+        # Disconnect the graph by promoting one edge to a different level.
+        edge = next(iter(simple_se2_graph.edges()))
+        edge.set_level(1)
+
+        assert not simple_se2_graph.is_connected(0)
+        assert simple_se2_graph.num_connected_components(0) == 2
+        assert not simple_se2_graph.is_connected(1)
+        assert simple_se2_graph.num_connected_components(1) == 2
 
 
 class TestBlockSolvers: