Re-add the IO tests for 2D SLAM

* implement a type based fixture that tests the IO of fixed size edges

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.

todo.md

@@ -3,6 +3,10 @@
 [ ] Re-work python wrapping for parameters, templated base param
 [ ] Test load/save for VertexCam (camera and baseline seem missing from traits)
 [ ] Refactor Data container. Can it be a container instead of linked list?
+[ ] Use FakeDependency in Traits
+[ ] EdgeSE2TwoPointsXY, EdgeSE2PointXYCalib can be fixed size edges -> IO test
+[ ] IO test fixture including parameter
+[ ] Type Based Tests for Jacobian possible?
 [x] support saving VectorX and MatrixX in G2O format
 [x] update python optimizable graph wrapper (save/load) with format
 [x] add version number for file format

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,40 @@
+// 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 <gtest/gtest.h>
+
+#include "g2o/types/slam2d/edge_se2.h"
+#include "g2o/types/slam2d/edge_se2_pointxy.h"
+#include "g2o/types/slam2d/edge_se2_pointxy_bearing.h"
+#include "g2o/types/slam2d/edge_se2_prior.h"
+#include "g2o/types/slam2d/edge_xy_prior.h"
+#include "unit_test/test_helper/typed_io.h"
+
+using Slam2DIoTypes = ::testing::Types<g2o::EdgeSE2, g2o::EdgeSE2PointXY,
+                                       g2o::EdgeSE2PointXYBearing,
+                                       g2o::EdgeSE2Prior, g2o::EdgeXYPrior>;
+INSTANTIATE_TYPED_TEST_SUITE_P(Slam2D, FixedSizeEdgeIO, Slam2DIoTypes,
+                               g2o::internal::DefaultTypeNames);

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,46 @@ struct RandomDouble {
   }
 };
 
-}  // namespace internal
-}  // namespace g2o
+template <typename T>
+struct RandomValue {
+  using Type = T;
+  template <typename FakeType>
+  struct FakeDependency : public std::false_type {};
+  static Type create() {
+    static_assert(FakeDependency<T>::value,
+                  "No specialization for RandomValue provided");
+    return T{};
+  }
+};
+
+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(); }
+};
+
+template <int N>
+struct RandomValue<g2o::VectorN<N>> {
+  using Type = g2o::VectorN<N>;
+  static Type create() { return g2o::VectorN<N>::Random(); }
+};
+
+}  // namespace g2o::internal

unit_test/test_helper/typed_io.h

@@ -0,0 +1,129 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2014 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 <string>
+
+#include "g2o/core/io/io_format.h"
+#include "g2o/core/optimizable_graph.h"
+#include "g2o/core/sparse_optimizer.h"
+#include "unit_test/test_helper/allocate_optimizer.h"
+#include "unit_test/test_helper/random_state.h"
+
+template <typename T>
+struct FixedSizeEdgeIO : public ::testing::Test {
+ public:
+  FixedSizeEdgeIO() = default;
+
+  void SetUp() override {
+    using EdgeType = T;
+
+    // Construct a small graph
+    auto edge = std::make_shared<EdgeType>();
+
+    // Initialize the vertices of the edge to a random state
+    for (size_t i = 0; i < edge->vertices().size(); ++i) {
+      auto v =
+          std::shared_ptr<g2o::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);
+    }
+
+    // Generate a random information matrix
+    const typename EdgeType::InformationType information_matrix = []() {
+      const typename EdgeType::InformationType aux =
+          EdgeType::InformationType::Random();
+      typename EdgeType::InformationType result = 0.5 * (aux + aux.transpose());
+      result.diagonal().array() += result.cols();
+      return result;
+    }();
+
+    edge->setInformation(information_matrix);
+    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(FixedSizeEdgeIO);
+
+TYPED_TEST_P(FixedSizeEdgeIO, SaveAndLoad) {
+  using namespace testing;  // NOLINT
+  constexpr g2o::io::Format kFormat = g2o::io::Format::kG2O;
+
+  std::stringstream buffer;
+  bool save_result = this->optimizer_ptr_->save(buffer, kFormat);
+  ASSERT_THAT(save_result, IsTrue());
+  EXPECT_THAT(buffer.str(), Not(IsEmpty()));
+
+  auto loaded_optimizer = g2o::internal::createOptimizerForTests();
+  loaded_optimizer->load(buffer, kFormat);
+
+  // Brutally check that serialization result is the same
+  std::stringstream buffer_after_loading;
+  save_result = loaded_optimizer->save(buffer_after_loading, kFormat);
+  ASSERT_THAT(save_result, IsTrue());
+  EXPECT_THAT(buffer.str(), Eq(buffer_after_loading.str()));
+}
+
+REGISTER_TYPED_TEST_SUITE_P(FixedSizeEdgeIO, SaveAndLoad);
+
+namespace g2o::internal {
+class DefaultTypeNames {
+ public:
+  template <typename T>
+  static std::string GetName(int) {
+    return testing::internal::GetTypeName<T>();
+  }
+};
+
+}  // namespace g2o::internal