Add graph summary print

Author: RainerKuemmerle

g2o/apps/g2o_cli/g2o.cpp

@@ -130,6 +130,7 @@ int main(int argc, char** argv) {
   std::string robustKernel;
   bool computeMarginals = false;
   bool printSolverProperties = false;
+  bool printGraphSummary = false;
   double huberWidth = -1.;
   double gain = 1e-6;
   int maxIterationsWithGain = std::numeric_limits<int>::max();
@@ -207,6 +208,8 @@ int main(int argc, char** argv) {
                  "specify a types library which will be loaded");
 #endif
   app.add_option("--stats", statsFile, "specify a file for the statistics");
+  app.add_flag("--graph_summary", printGraphSummary,
+               "print a short summary of the loaded graph and exit");
   app.add_flag("--list_types", listTypes, "list the registered types");
   app.add_flag("--list_robust_kernels", listRobustKernels,
                "list the registered robust kernels");
@@ -316,6 +319,11 @@ int main(int argc, char** argv) {
   cerr << "Loaded " << optimizer.vertices().size() << " vertices\n";
   cerr << "Loaded " << optimizer.edges().size() << " edges\n";
 
+  if (printGraphSummary) {
+    optimizer.printGraphSummary(cout);
+    return 0;
+  }
+
   if (optimizer.vertices().empty()) {
     cerr << "Graph contains no vertices\n";
     return 1;

g2o/core/sparse_optimizer.cpp

@@ -128,6 +128,44 @@ double SparseOptimizer::activeRobustChi2() const {
   return chi;
 }
 
+void SparseOptimizer::printGraphSummary(std::ostream& os) const {
+  const size_t nVertices = vertices().size();
+  const size_t nEdges = edges().size();
+  os << "Graph summary:\n";
+  os << "  vertices: " << nVertices << "\n";
+  os << "  edges: " << nEdges << "\n";
+
+  if (nVertices > 0) {
+    const int maxDim = maxDimension();
+    int nPoses = 0;
+    int nLandmarks = 0;
+    std::unordered_set<int> levels;
+    for (const auto& it : vertices()) {
+      auto v = std::static_pointer_cast<OptimizableGraph::Vertex>(it.second);
+      if (v->dimension() == maxDim)
+        nPoses++;
+      else
+        nLandmarks++;
+    }
+    for (const auto& e : edges()) {
+      auto oe = static_cast<OptimizableGraph::Edge*>(e.get());
+      if (oe) {
+        levels.insert(oe->level());
+      }
+    }
+    os << "  poses: " << nPoses << "\n";
+    os << "  landmarks: " << nLandmarks << "\n";
+    os << "  levels: " << levels.size() << "\n";
+  }
+
+  os << "  active vertices: " << activeVertices_.size() << "\n";
+  os << "  active edges: " << activeEdges_.size() << "\n";
+  if (!activeEdges_.empty()) {
+    os << "  active chi2: " << activeChi2() << "\n";
+    os << "  active robust chi2: " << activeRobustChi2() << "\n";
+  }
+}
+
 std::shared_ptr<OptimizableGraph::Vertex> SparseOptimizer::findGauge() {
   if (vertices().empty()) return nullptr;
 

g2o/core/sparse_optimizer.h

@@ -28,6 +28,7 @@
 #define G2O_GRAPH_OPTIMIZER_CHOL_H_
 
 #include <memory>
+#include <ostream>
 #include <utility>
 #include <vector>
 
@@ -171,6 +172,11 @@ class G2O_CORE_API SparseOptimizer : public OptimizableGraph {
    */
   double activeRobustChi2() const;
 
+  /**
+   * Print a short summary of the currently loaded graph.
+   */
+  void printGraphSummary(std::ostream& os = std::cout) const;
+
   //! verbose information during optimization
   bool verbose() const { return verbose_; }
   void setVerbose(bool verbose);

python/core/py_sparse_optimizer.cpp

@@ -10,6 +10,7 @@
 #include <g2o/core/optimization_algorithm_with_hessian.h>
 #include <g2o/core/sparse_optimizer.h>
 
+#include <sstream>
 #include <utility>
 
 namespace g2o {
@@ -83,7 +84,13 @@ 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("verbose", &CLS::verbose)                      // -> bool
+      .def("print_graph_summary",
+           [](CLS& optimizer) {
+             std::ostringstream os;
+             optimizer.printGraphSummary(os);
+             return os.str();
+           })
+      .def("verbose", &CLS::verbose)  // -> bool
       .def("set_verbose", &CLS::setVerbose,
            "verbose"_a)  // -> void
 

python/tests/test_core.py

@@ -51,6 +51,13 @@ def test_set_verbose(self, basic_se2_optimizer):
         basic_se2_optimizer.set_verbose(False)
         # Should not raise an exception
 
+    def test_print_graph_summary(self, simple_se2_graph):
+        """Test Python binding for graph summary."""
+        summary = simple_se2_graph.print_graph_summary()
+        assert "vertices:" in summary
+        assert "edges:" in summary
+        assert "poses:" in summary
+
 
 class TestBlockSolvers:
     """Test BlockSolver configurations."""

unit_test/general/graph_operations.cpp

@@ -31,6 +31,7 @@
 #include <cstddef>
 #include <memory>
 #include <numeric>
+#include <sstream>
 #include <unordered_set>
 #include <utility>
 #include <vector>
@@ -152,6 +153,69 @@ TEST(General, GraphAddEdge) {
   ASSERT_TRUE(optimizer->edges().empty());
 }
 
+TEST(General, GraphPrintSummary) {
+  auto optimizer = g2o::internal::createOptimizerForTests();
+
+  auto v0 = std::make_shared<g2o::VertexSE2>();
+  v0->setId(0);
+  auto v1 = std::make_shared<g2o::VertexSE2>();
+  v1->setId(1);
+
+  ASSERT_TRUE(optimizer->addVertex(v0));
+  ASSERT_TRUE(optimizer->addVertex(v1));
+
+  auto e0 = std::make_shared<g2o::EdgeSE2>();
+  e0->setVertex(0, v0);
+  e0->setVertex(1, v1);
+  e0->setLevel(0);
+  ASSERT_TRUE(optimizer->addEdge(e0));
+
+  std::ostringstream os;
+  optimizer->printGraphSummary(os);
+  const std::string summary = os.str();
+
+  EXPECT_NE(summary.find("vertices: 2"), std::string::npos);
+  EXPECT_NE(summary.find("edges: 1"), std::string::npos);
+  EXPECT_NE(summary.find("levels: 1"), std::string::npos);
+  EXPECT_NE(summary.find("connected_components: 1"), std::string::npos);
+}
+
+TEST(General, GraphConnectedComponents) {
+  auto optimizer = g2o::internal::createOptimizerForTests();
+
+  auto v0 = std::make_shared<g2o::VertexSE2>();
+  v0->setId(0);
+  auto v1 = std::make_shared<g2o::VertexSE2>();
+  v1->setId(1);
+  auto v2 = std::make_shared<g2o::VertexSE2>();
+  v2->setId(2);
+
+  ASSERT_TRUE(optimizer->addVertex(v0));
+  ASSERT_TRUE(optimizer->addVertex(v1));
+  ASSERT_TRUE(optimizer->addVertex(v2));
+
+  auto e01 = std::make_shared<g2o::EdgeSE2>();
+  e01->setVertex(0, v0);
+  e01->setVertex(1, v1);
+  e01->setLevel(0);
+  ASSERT_TRUE(optimizer->addEdge(e01));
+
+  auto e12 = std::make_shared<g2o::EdgeSE2>();
+  e12->setVertex(0, v1);
+  e12->setVertex(1, v2);
+  e12->setLevel(1);
+  ASSERT_TRUE(optimizer->addEdge(e12));
+
+  EXPECT_FALSE(optimizer->isConnected(0));
+  EXPECT_EQ(optimizer->numConnectedComponents(0), 2);
+
+  EXPECT_FALSE(optimizer->isConnected(1));
+  EXPECT_EQ(optimizer->numConnectedComponents(1), 2);
+
+  EXPECT_FALSE(optimizer->isConnected(2));
+  EXPECT_EQ(optimizer->numConnectedComponents(2), 3);
+}
+
 TEST(General, GraphIndexMapping) {
   auto optimizer = g2o::internal::createOptimizerForTests();