WIP Re-add the IO tests

g2o/core/abstract_graph.cpp

@@ -37,6 +37,7 @@
 #include "io/io_g2o.h"
 
 namespace {
+#ifdef G2O_HAVE_LOGGING
 std::string_view to_string(g2o::io::Format format) {
   switch (format) {
     case g2o::io::Format::kG2O:
@@ -50,6 +51,7 @@ std::string_view to_string(g2o::io::Format format) {
   }
   return "";
 }
+#endif
 
 /**
  * @brief Allocate an g2o::IoInterface to load/save data of the graph.

unit_test/slam2d/CMakeLists.txt

@@ -1,6 +1,7 @@
 add_executable(unittest_slam2d
   mappings_se2.cpp
   jacobians_slam2d.cpp
+  slam2d_io.cpp
   solve_direct_slam2d.cpp
 )
 target_link_libraries(unittest_slam2d unittest_helper types_slam2d)

unit_test/slam2d/slam2d_io.cpp

@@ -0,0 +1,116 @@
+// 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 <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "g2o/core/io/io_format.h"
+#include "g2o/core/optimizable_graph.h"
+#include "g2o/core/sparse_optimizer.h"
+#include "g2o/types/slam2d/edge_se2.h"
+#include "unit_test/test_helper/allocate_optimizer.h"
+#include "unit_test/test_helper/random_state.h"
+
+using namespace g2o;      // NOLINT
+using namespace testing;  // NOLINT
+
+template <typename T>
+struct FixedSizeEdgeIOFixture : public ::testing::Test {
+ public:
+  FixedSizeEdgeIOFixture() = default;
+
+  void SetUp() override {
+    using EdgeType = T;
+
+    // Construct a small graph
+    auto edge = std::make_shared<EdgeType>();
+
+    for (size_t i = 0; i < edge->vertices().size(); ++i) {
+      auto v = std::shared_ptr<OptimizableGraph::Vertex>(edge->createVertex(i));
+      edge->vertices()[i] = v;
+      v->setId(i);
+      v->setFixed(i == 0);
+      initializeNthVertex<EdgeType::kNrOfVertices - 1>(i, *edge);
+      this->optimizer_ptr_->addVertex(v);
+    }
+
+    // TODO(Rainer): better randomization for information matrix (off-diag
+    // elemns)
+    VectorN<EdgeType::InformationType::ColsAtCompileTime> info_diagonal =
+        VectorN<EdgeType::InformationType::ColsAtCompileTime>::Random();
+    edge->setInformation(info_diagonal.cwiseAbs().asDiagonal());
+    edge->setMeasurement(
+        g2o::internal::RandomValue<typename EdgeType::Measurement>::create());
+    this->optimizer_ptr_->addEdge(edge);
+  }
+
+ protected:
+  std::unique_ptr<g2o::SparseOptimizer> optimizer_ptr_ =
+      g2o::internal::createOptimizerForTests();
+
+  template <int I, typename EdgeType>
+  static std::enable_if_t<I == -1, void> initializeNthVertex(int /*i*/,
+                                                             EdgeType& /*t*/) {}
+
+  template <int I, typename EdgeType>
+  static std::enable_if_t<I != -1, void> initializeNthVertex(int i,
+                                                             EdgeType& t) {
+    if (i == I) {
+      auto vertex = t.template vertexXn<I>();
+      vertex->setEstimate(
+          g2o::internal::RandomValue<typename EdgeType::template VertexXnType<
+              I>::EstimateType>::create());
+      return;
+    }
+    initializeNthVertex<I - 1, EdgeType>(i, t);
+  }
+};
+TYPED_TEST_SUITE_P(FixedSizeEdgeIOFixture);
+
+TYPED_TEST_P(FixedSizeEdgeIOFixture, SaveAndLoad) {
+  g2o::io::Format format = io::Format::kG2O;
+
+  std::stringstream buffer;
+  bool save_result = this->optimizer_ptr_->save(buffer, format);
+  ASSERT_THAT(save_result, IsTrue());
+  EXPECT_THAT(buffer.str(), Not(IsEmpty()));
+
+  auto loaded_optimizer = g2o::internal::createOptimizerForTests();
+  loaded_optimizer->load(buffer, format);
+
+  // Brutally check that serialization result is the same
+  std::stringstream buffer_after_loading;
+  save_result = loaded_optimizer->save(buffer_after_loading, format);
+  ASSERT_THAT(save_result, IsTrue());
+  EXPECT_THAT(buffer.str(), Eq(buffer_after_loading.str()));
+}
+
+REGISTER_TYPED_TEST_SUITE_P(FixedSizeEdgeIOFixture, SaveAndLoad);
+
+using Slam2DIoTypes = ::testing::Types<EdgeSE2>;
+INSTANTIATE_TYPED_TEST_SUITE_P(Slam2D, FixedSizeEdgeIOFixture, Slam2DIoTypes);

unit_test/test_helper/random_state.h

@@ -26,13 +26,12 @@
 
 #include <Eigen/Geometry>
 
-#include "g2o/config.h"
 #include "g2o/core/eigen_types.h"
 #include "g2o/stuff/sampler.h"
+#include "g2o/types/slam2d/se2.h"
 #include "g2o/types/slam3d/se3quat.h"
 
-namespace g2o {
-namespace internal {
+namespace g2o::internal {
 inline Isometry3 randomIsometry3() {
   const g2o::Vector3 rotAxisAngle =
       g2o::Vector3::Random() + g2o::Vector3::Random();
@@ -65,5 +64,36 @@ struct RandomDouble {
   }
 };
 
-}  // namespace internal
-}  // namespace g2o
+template <typename T>
+struct RandomValue {
+  // using Type = T;
+  // static Type create() {
+  //   static_assert(false, "No specialization provided");
+  // }
+};
+
+template <>
+struct RandomValue<double> {
+  using Type = double;
+  static Type create() { return RandomDouble::create(); }
+};
+
+template <>
+struct RandomValue<g2o::SE2> {
+  using Type = g2o::SE2;
+  static Type create() { return Type(g2o::Vector3::Random()); }
+};
+
+template <>
+struct RandomValue<g2o::SE3Quat> {
+  using Type = g2o::SE3Quat;
+  static Type create() { return RandomSE3Quat::create(); }
+};
+
+template <>
+struct RandomValue<g2o::Isometry3> {
+  using Type = g2o::Isometry3;
+  static Type create() { return RandomIsometry3::create(); }
+};
+
+}  // namespace g2o::internal