Start to add test for save/load in g2o format

g2o/core/CMakeLists.txt

@@ -40,6 +40,7 @@ robust_kernel_factory.cpp robust_kernel_factory.h
 io_helper.h
 g2o_core_api.h
 # IO
+abstract_graph.cpp abstract_graph.h
 io/io_g2o.cpp io/io_g2o.h
 )
 

g2o/core/abstract_graph.cpp

@@ -43,7 +43,8 @@ std::unique_ptr<g2o::IoInterface> allocate(g2o::AbstractGraph::Format format) {
     case g2o::AbstractGraph::Format::kXML:
       break;
   }
-  G2O_CRITICAL("Failed to create graph loader interface for format {}", format);
+  G2O_CRITICAL("Failed to create graph loader interface for format {}",
+               static_cast<int>(format));
   return nullptr;
 }
 }  // namespace

g2o/core/abstract_graph.h

@@ -29,6 +29,7 @@
 
 #include <istream>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace g2o {
@@ -49,29 +50,53 @@ class AbstractGraph {
   struct AbstractData {
     std::string tag;   ///< the tag of this data
     std::string data;  ///< the serialized data as a string
+    AbstractData() = default;
+    AbstractData(std::string tag, std::string data)
+        : tag(std::move(tag)), data(std::move(data)) {}
   };
 
   struct AbstractParameter {
     std::string tag;            ///< the tag of this parameter
     int id;                     ///< its ID
     std::vector<double> value;  ///< its value as a vector
+    AbstractParameter() = default;
+    AbstractParameter(std::string tag, int id, std::vector<double> value)
+        : tag(std::move(tag)), id(id), value(std::move(value)) {}
   };
 
   struct AbstractGraphElement {
     std::string tag;
     std::vector<AbstractData> data;  ///< the associated data
+    AbstractGraphElement() = default;
+    AbstractGraphElement(std::string tag, std::vector<AbstractData> data)
+        : tag(std::move(tag)), data(std::move(data)) {}
   };
 
   struct AbstractVertex : public AbstractGraphElement {
     int id;                        ///< its ID
     std::vector<double> estimate;  ///< the estimate as a vector
+    AbstractVertex() = default;
+    AbstractVertex(std::string tag, int id, std::vector<double> estimate,
+                   std::vector<AbstractData> data = {})
+        : AbstractGraphElement(std::move(tag), std::move(data)),
+          id(id),
+          estimate(std::move(estimate)) {}
   };
 
   struct AbstractEdge : public AbstractGraphElement {
     std::vector<int> ids;  ///< the ids of the vertices connected by this edge
     std::vector<double> measurement;  ///< the measurement as a vector
     std::vector<double>
         information;  ///< upper triangular part of the information matrix
+    AbstractEdge() = default;
+    AbstractEdge(std::string tag, std::vector<int> ids,
+                 std::vector<double> measurement,
+                 std::vector<double> information,
+                 std::vector<AbstractData> data = {})
+        : AbstractGraphElement(std::move(tag), std::move(data)),
+          ids(std::move(ids)),
+          measurement(std::move(measurement)),
+          information(std::move(information)) {}
   };
 
   //! Possible formats for loading and saving
@@ -84,10 +109,10 @@ class AbstractGraph {
             Format format = AbstractGraph::Format::kG2O);
   bool load(std::istream& input, Format format = AbstractGraph::Format::kG2O);
 
-  bool save(const std::string& filename,
-            Format format = AbstractGraph::Format::kG2O) const;
-  bool save(std::ostream& output,
-            Format format = AbstractGraph::Format::kG2O) const;
+  [[nodiscard]] bool save(const std::string& filename,
+                          Format format = AbstractGraph::Format::kG2O) const;
+  [[nodiscard]] bool save(std::ostream& output,
+                          Format format = AbstractGraph::Format::kG2O) const;
 
   std::vector<int>& fixed() { return fixed_; };
   [[nodiscard]] const std::vector<int>& fixed() const { return fixed_; };

g2o/core/io/io_g2o.cpp

@@ -177,7 +177,7 @@ AbstractGraph IoG2O::load(std::istream& input) {
 
 bool IoG2O::save(std::ostream& output, const AbstractGraph& graph) {
   if (!graph.fixed().empty()) {
-    output << "FIX" << graph.fixed() << '\n';
+    output << "FIX " << graph.fixed() << '\n';
   }
 
   for (const auto& param : graph.parameters()) {

unit_test/general/CMakeLists.txt

@@ -1,12 +1,14 @@
 add_executable(unittest_general
   auto_diff.cpp
   batch_statistics.cpp
+  graph_io.cpp
   graph_operations.cpp
   clear_and_redo.cpp
   base_fixed_sized_edge.cpp
   robust_kernel_tests.cpp
   sparse_block_matrix.cpp
   type_traits_tests.cpp
 )
-target_link_libraries(unittest_general unittest_helper types_slam3d types_slam2d)
+target_link_libraries(unittest_general unittest_helper)
+target_link_libraries(unittest_general types_slam3d types_slam2d types_data)
 create_test(unittest_general)

unit_test/general/auto_diff.cpp

@@ -165,12 +165,6 @@ class AutoDifferentiation : public ::testing::Test {
 TEST_F(AutoDifferentiation, ComputesSomething) {
   testEdge_.linearizeOplus(jacobianWorkspace_);
 
-#if 0
-  std::cerr << PVAR(testEdge.jacobianOplusXn<0>()) << std::endl;
-  std::cerr << PVAR(testEdge.jacobianOplusXn<1>()) << std::endl;
-  std::cerr << PVAR(testEdge.jacobianOplusXn<2>()) << std::endl;
-#endif
-
   ASSERT_FALSE(testEdge_.jacobianOplusXn<0>().array().isNaN().any())
       << "Jacobian contains NaN";
   ASSERT_FALSE(testEdge_.jacobianOplusXn<1>().array().isNaN().any())
@@ -242,11 +236,6 @@ class AutoDifferentiationEdgeSE2 : public ::testing::Test {
 TEST_F(AutoDifferentiationEdgeSE2, AdComputesSomething) {
   testEdgeAd_.linearizeOplus(jacobianWorkspaceAd_);
 
-#if 0
-  std::cerr << PVAR(testEdgeAd.jacobianOplusXn<0>()) << std::endl;
-  std::cerr << PVAR(testEdgeAd.jacobianOplusXn<1>()) << std::endl;
-#endif
-
   ASSERT_FALSE(testEdgeAd_.jacobianOplusXn<0>().array().isNaN().any())
       << "Jacobian contains NaN";
   ASSERT_FALSE(testEdgeAd_.jacobianOplusXn<1>().array().isNaN().any())

unit_test/general/graph_io.cpp

@@ -0,0 +1,104 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <sstream>
+
+#include "g2o/core/abstract_graph.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+using namespace testing;  // NOLINT
+
+MATCHER(ParamEqual, "") {
+  const auto& that = std::get<0>(arg);
+  const auto& expected = std::get<1>(arg);
+  return ExplainMatchResult(
+      AllOf(Field("tag", &g2o::AbstractGraph::AbstractParameter::tag,
+                  Eq(expected.tag)),
+            Field("id", &g2o::AbstractGraph::AbstractParameter::id,
+                  Eq(expected.id)),
+            Field("value", &g2o::AbstractGraph::AbstractParameter::value,
+                  ElementsAreArray(expected.value))),
+      that, result_listener);
+}
+
+/**
+ * @brief Test fixture for IO with the abstract graph.
+ */
+class AbstractGraphIO : public TestWithParam<g2o::AbstractGraph::Format> {
+ protected:
+  void SetUp() override {
+    abstract_graph_.fixed() = {1};
+
+    abstract_graph_.parameters().emplace_back("PARAMS_SE2OFFSET", 42,
+                                              std::vector<double>{12., 13.});
+
+    abstract_graph_.vertices().emplace_back("VERTEX_SE2", 1,
+                                            std::vector<double>{1., 2., 3.});
+    abstract_graph_.vertices().emplace_back("VERTEX_SE2", 2,
+                                            std::vector<double>{2., 3., 4.});
+    abstract_graph_.vertices().emplace_back(
+        "VERTEX_SE2", 3, std::vector<double>{5., 6., 7.},
+        std::vector<g2o::AbstractGraph::AbstractData>{
+            {"VERTEX_TAG", "fancy data"}});
+
+    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<g2o::AbstractGraph::AbstractData>{
+            {"VERTEX_TAG", "more fancy data"}});
+    abstract_graph_.edges().emplace_back(
+        "EDGE_SE2", std::vector<int>{2, 3}, std::vector<double>{0.4, 0.5, 0.6},
+        std::vector<double>{1.1, 2.1, 3.1, 4.1, 5.1, 6.1});
+  }
+  g2o::AbstractGraph abstract_graph_;
+};
+
+TEST_P(AbstractGraphIO, SaveAndLoad) {
+  g2o::AbstractGraph::Format format = GetParam();
+  g2o::AbstractGraph load_save_graph(abstract_graph_);
+
+  std::stringstream buffer;
+  bool save_result = load_save_graph.save(buffer, format);
+  ASSERT_THAT(save_result, IsTrue());
+
+  load_save_graph.clear();
+  EXPECT_THAT(load_save_graph.fixed(), IsEmpty());
+  EXPECT_THAT(load_save_graph.parameters(), IsEmpty());
+  EXPECT_THAT(load_save_graph.vertices(), IsEmpty());
+  EXPECT_THAT(load_save_graph.edges(), IsEmpty());
+
+  bool load_status = load_save_graph.load(buffer, format);
+  ASSERT_THAT(load_status, IsTrue());
+
+  EXPECT_THAT(load_save_graph.fixed(),
+              ElementsAreArray(abstract_graph_.fixed()));
+  EXPECT_THAT(load_save_graph.parameters(),
+              Pointwise(ParamEqual(), abstract_graph_.parameters()));
+}
+
+INSTANTIATE_TEST_SUITE_P(AbstractGraph, AbstractGraphIO,
+                         Values(g2o::AbstractGraph::Format::kG2O));