Add save/load of edges' level

Relates #954

Author: RainerKuemmerle

g2o/core/abstract_graph.h

@@ -93,17 +93,20 @@ class G2O_CORE_API AbstractGraph {
     std::vector<double> measurement;  ///< the measurement as a vector
     std::vector<double>
         information;  ///< upper triangular part of the information matrix
+    int level =
+        0;  ///< the level of this edge, used for hierarchical optimization
     AbstractEdge() = default;
     AbstractEdge(std::string tag, std::vector<int> ids,
                  std::vector<double> measurement,
                  std::vector<double> information,
-                 std::vector<int> param_ids = {},
+                 std::vector<int> param_ids = {}, int level = 0,
                  std::vector<AbstractData> data = {})
         : AbstractGraphElement(std::move(tag), std::move(data)),
           ids(std::move(ids)),
           param_ids(std::move(param_ids)),
           measurement(std::move(measurement)),
-          information(std::move(information)) {}
+          information(std::move(information)),
+          level(level) {}
   };
 
   AbstractGraph() = default;

g2o/core/io/io_g2o.cpp

@@ -50,6 +50,8 @@ std::ostream& operator<<(std::ostream& os, const std::vector<T>& v) {
   }
   return os;
 }
+
+std::unordered_set<std::string> kInternalTokens = {"G2O_EDGE_LEVEL"};
 }  // namespace
 
 namespace g2o {
@@ -83,6 +85,27 @@ std::optional<AbstractGraph> IoG2O::load(std::istream& input) {
       continue;
     }
 
+    if (kInternalTokens.count(token) > 0) {
+      // handle internal tokens that are not part of the graph, but control
+      // loading/saving behavior
+      if (token == "G2O_EDGE_LEVEL") {
+        int level;
+        if (!(current_line >> level)) {
+          G2O_ERROR("Error reading edge level at line {}", line_number);
+          continue;
+        }
+        G2O_TRACE("Setting edge level to {} for last edge", level);
+        if (result.edges().empty()) {
+          G2O_ERROR(
+              "Got edge level {}, but no edge has been read yet at line {}",
+              level, line_number);
+          continue;
+        }
+        result.edges().back().level = level;
+      }
+      continue;
+    }
+
     token = mapType(token);
     G2O_TRACE("Reading token {}", token);
     Factory::TypeInfo type_info = factory->typeInfo(token);
@@ -232,6 +255,9 @@ bool IoG2O::save(std::ostream& output, const AbstractGraph& graph) {
       output << edge.information.size() << " ";
     output << edge.information << "\n";
     printData(output, edge.data);
+    if (edge.level != 0) {
+      output << "G2O_EDGE_LEVEL " << edge.level << "\n";
+    }
   }
 
   // After the vertices to be backward compatible

g2o/core/io/io_wrapper_json.h

@@ -115,6 +115,9 @@ inline void to_json(nlohmann::json& j,
                      {"ids", edge.ids},
                      {"measurement", edge.measurement},
                      {"information", edge.information}};
+  if (edge.level != 0) {
+    j["level"] = edge.level;
+  }
   internal::store_if_not_empty(j, "data", edge.data);
   internal::store_if_not_empty(j, "param_ids", edge.param_ids);
 }
@@ -125,6 +128,7 @@ inline void from_json(const nlohmann::json& j,
   j.at("ids").get_to(edge.ids);
   j.at("measurement").get_to(edge.measurement);
   j.at("information").get_to(edge.information);
+  internal::get_to_if_exists(j, "level", edge.level);
   internal::get_to_if_exists(j, "data", edge.data);
   internal::get_to_if_exists(j, "param_ids", edge.param_ids);
 }

g2o/core/optimizable_graph.cpp

@@ -455,6 +455,7 @@ bool OptimizableGraph::load(std::istream& is, io::Format format) {
         information(r, c) = abstract_edge.information[idx++];
         if (r != c) information(c, r) = information(r, c);
       }
+    edge->setLevel(abstract_edge.level);
     if (!addEdge(edge)) {
       G2O_ERROR(
           "Failure adding Edge {} IDs {}", abstract_edge.tag,
@@ -728,7 +729,7 @@ bool OptimizableGraph::saveEdge(AbstractGraph& abstract_graph,
     for (int c = r; c < e->dimension(); ++c)
       upper_triangle.push_back(information(r, c));
   abstract_graph.edges().emplace_back(tag, ids, data, upper_triangle,
-                                      e->parameterIds());
+                                      e->parameterIds(), e->level());
   saveUserData(abstract_graph.edges().back(), e->userData());
   return true;
 }

unit_test/general/graph_io.cpp

@@ -84,6 +84,8 @@ MATCHER(EdgeEqual, "") {
                   Eq(expected.tag)),
             Field("ids", &g2o::AbstractGraph::AbstractEdge::ids,
                   ElementsAreArray(expected.ids)),
+            Field("level", &g2o::AbstractGraph::AbstractEdge::level,
+                  Eq(expected.level)),
             Field("param_ids", &g2o::AbstractGraph::AbstractEdge::param_ids,
                   ElementsAreArray(expected.param_ids)),
             Field("measurement", &g2o::AbstractGraph::AbstractEdge::measurement,
@@ -124,7 +126,7 @@ class AbstractGraphIO : public TestWithParam<g2o::io::Format> {
 
     abstract_graph_.edges().emplace_back(
         "EDGE_SE2", std::vector<int>{1, 2}, std::vector<double>{0.1, 0.2, 0.3},
-        std::vector<double>{1., 2., 3., 4., 5., 6.}, std::vector<int>(),
+        std::vector<double>{1., 2., 3., 4., 5., 6.}, std::vector<int>(), 1,
         std::vector<g2o::AbstractGraph::AbstractData>{
             {"VERTEX_TAG", "more fancy data"},
             {"VERTEX_TAG", "even more fancy data"}});
@@ -247,6 +249,7 @@ class OptimizableGraphIO : public TestWithParam<g2o::io::Format> {
     e2->vertices()[1] = optimizer_ptr_->vertex(2);
     e2->setMeasurement(g2o::SE2(0, 1, 0));
     e2->setInformation(g2o::MatrixN<3>::Identity());
+    e2->setLevel(0);
     optimizer_ptr_->addEdge(e2);
   }
   std::unique_ptr<g2o::SparseOptimizer> optimizer_ptr_;
@@ -305,6 +308,44 @@ TEST_P(OptimizableGraphIO, SaveAndLoad) {
   EXPECT_THAT(buffer.str(), Eq(buffer_after_loading.str()));
 }
 
+TEST_P(OptimizableGraphIO, SaveAndLoadWithDifferentEdgeLevels) {
+  g2o::io::Format format = GetParam();
+
+  // Manually set different levels for the edges
+  auto edge_iter = optimizer_ptr_->edges().begin();
+  static_cast<g2o::OptimizableGraph::Edge*>(edge_iter->get())->setLevel(2);
+  ++edge_iter;
+  static_cast<g2o::OptimizableGraph::Edge*>(edge_iter->get())->setLevel(5);
+
+  std::stringstream buffer(format == g2o::io::Format::kBinary
+                               ? std::ios_base::binary | std::ios_base::in |
+                                     std::ios_base::out
+                               : std::ios_base::in | std::ios_base::out);
+  constexpr int kLevelToSave = 2;
+  bool save_result = optimizer_ptr_->save(buffer, format, kLevelToSave);
+  ASSERT_THAT(save_result, IsTrue());
+  EXPECT_THAT(buffer.str(), Not(IsEmpty()));
+
+  auto loaded_optimizer = g2o::internal::createOptimizerForTests();
+  loaded_optimizer->load(buffer, format);
+
+  EXPECT_THAT(loaded_optimizer->vertices(), Not(IsEmpty()));
+  EXPECT_THAT(loaded_optimizer->edges(), Not(IsEmpty()));
+
+  for (const auto& edge_ptr : loaded_optimizer->edges()) {
+    ASSERT_NE(edge_ptr, nullptr);
+    EXPECT_THAT(
+        static_cast<g2o::OptimizableGraph::Edge*>(edge_ptr.get())->level(),
+        Eq(kLevelToSave));
+  }
+
+  std::stringstream buffer_after_loading;
+  save_result =
+      loaded_optimizer->save(buffer_after_loading, format, kLevelToSave);
+  ASSERT_THAT(save_result, IsTrue());
+  EXPECT_THAT(buffer.str(), Eq(buffer_after_loading.str()));
+}
+
 namespace {
 // We can always test G2O format, others depend on libraries
 const auto kFileformatsToTest =