WIP Re-add the IO tests

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,95 @@
+// 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 BinaryEdgeIOFixture : public ::testing::Test {
+ public:
+  BinaryEdgeIOFixture() = default;
+
+  void SetUp() override {
+    using BinaryEdgeType = T;
+
+    // Construct a small graph
+    auto edge = std::make_shared<BinaryEdgeType>();
+
+    for (size_t i = 0; i < edge->vertices().size(); ++i) {
+      auto v = std::shared_ptr<OptimizableGraph::Vertex>(edge->createVertex(i));
+      v->setId(i);
+      // v->setEstimate(g2o::Isometry3::Identity());
+      v->setFixed(i == 0);
+      this->optimizer_ptr_->addVertex(v);
+      edge->vertices()[i] = v;
+    }
+
+    edge->setInformation(BinaryEdgeType::InformationType::Identity());
+    edge->setMeasurement(g2o::internal::RandomValue<
+                         typename BinaryEdgeType::Measurement>::create());
+    this->optimizer_ptr_->addEdge(edge);
+  }
+
+ protected:
+  std::unique_ptr<g2o::SparseOptimizer> optimizer_ptr_ =
+      g2o::internal::createOptimizerForTests();
+};
+TYPED_TEST_SUITE_P(BinaryEdgeIOFixture);
+
+TYPED_TEST_P(BinaryEdgeIOFixture, 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(BinaryEdgeIOFixture, SaveAndLoad);
+
+using Slam2DIoTypes = ::testing::Types<EdgeSE2>;
+INSTANTIATE_TYPED_TEST_SUITE_P(Slam2D, BinaryEdgeIOFixture, 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,37 @@ struct RandomDouble {
   }
 };
 
-}  // namespace internal
-}  // namespace g2o
+template <typename T>
+struct RandomValue {
+  using Type = T;
+  static Type create() {
+    static_assert(false, "No specialization provided");
+    return T();
+  }
+};
+
+template <>
+struct RandomValue<double> {
+  using Type = double;
+  static Type create() { return RandomDouble::create(); }
+};
+
+template <>
+struct g2o::internal::RandomValue<SE2> {
+  using Type = SE2;
+  static Type create() { return SE2(Vector3::Random()); }
+};
+
+template <>
+struct RandomValue<SE3Quat> {
+  using Type = SE3Quat;
+  static Type create() { return RandomSE3Quat::create(); }
+};
+
+template <>
+struct RandomValue<Isometry3> {
+  using Type = Isometry3;
+  static Type create() { return RandomIsometry3::create(); }
+};
+
+}  // namespace g2o::internal