Extend typed IO tests to support optional parameters

g2o/stuff/tuple_tools.h

@@ -30,15 +30,32 @@
 
 namespace g2o {
 
+namespace internal {
+template <std::size_t... Ns, typename... Ts>
+auto tail_impl(std::index_sequence<Ns...>, std::tuple<Ts...> t) {
+  return std::make_tuple(std::get<Ns + 1U>(t)...);
+}
+
 template <typename F, typename T, std::size_t... I>
-void tuple_apply_i_(F&& f, T& t, int i, std::index_sequence<I...>) {
+void tuple_apply_i_impl(F&& f, T& t, int i, std::index_sequence<I...>) {
   (..., (I == i ? f(std::get<I>(t)) : void()));
 }
+}  // namespace internal
 
 template <typename F, typename T>
 void tuple_apply_i(F&& f, T& t, int i) {
-  tuple_apply_i_(
+  internal::tuple_apply_i_impl(
       f, t, i, std::make_index_sequence<std::tuple_size_v<std::decay_t<T>>>());
 }
 
+template <typename T, typename... Ts>
+auto tupple_head(std::tuple<T, Ts...> t) {
+  return std::get<0>(t);
+}
+
+template <typename... Ts>
+auto tuple_tail(std::tuple<Ts...> t) {
+  return internal::tail_impl(std::make_index_sequence<sizeof...(Ts) - 1U>(), t);
+}
+
 }  // namespace g2o

todo.md

@@ -1,4 +1,5 @@
 [x] Create parameter type for ParameterCamera to support load/save
+[ ] Add typing for parameters to template?
 [ ] 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?

unit_test/general/clear_and_redo.cpp

@@ -28,7 +28,8 @@
 
 #include "g2o/core/sparse_optimizer.h"
 #include "g2o/core/sparse_optimizer_terminate_action.h"
-#include "g2o/types/slam3d/types_slam3d.h"
+#include "g2o/types/slam3d/edge_se3.h"
+#include "g2o/types/slam3d/vertex_se3.h"
 #include "unit_test/test_helper/allocate_optimizer.h"
 
 TEST(General, ClearAndRedo) {

unit_test/sba/sba_io.cpp

@@ -51,11 +51,12 @@ struct g2o::internal::RandomValue<g2o::SBACam> {
   }
 };
 
-using SBAIoTypes =
-    ::testing::Types<g2o::EdgeSE3Expmap, g2o::EdgeSBAScale, g2o::EdgeSBACam,
-                     g2o::EdgeSE3ProjectXYZ, g2o::EdgeSE3ProjectXYZOnlyPose,
-                     g2o::EdgeStereoSE3ProjectXYZ,
-                     g2o::EdgeStereoSE3ProjectXYZOnlyPose, g2o::EdgeProjectP2SC,
-                     g2o::EdgeProjectP2MC>;
+using SBAIoTypes = ::testing::Types<
+    std::tuple<g2o::EdgeSE3Expmap>, std::tuple<g2o::EdgeSBAScale>,
+    std::tuple<g2o::EdgeSBACam>, std::tuple<g2o::EdgeSE3ProjectXYZ>,
+    std::tuple<g2o::EdgeSE3ProjectXYZOnlyPose>,
+    std::tuple<g2o::EdgeStereoSE3ProjectXYZ>,
+    std::tuple<g2o::EdgeStereoSE3ProjectXYZOnlyPose>,
+    std::tuple<g2o::EdgeProjectP2SC>, std::tuple<g2o::EdgeProjectP2MC>>;
 INSTANTIATE_TYPED_TEST_SUITE_P(SBA, FixedSizeEdgeIO, SBAIoTypes,
                                g2o::internal::DefaultTypeNames);

unit_test/slam2d/slam2d_io.cpp

@@ -26,15 +26,26 @@
 
 #include <gtest/gtest.h>
 
+#include <tuple>
+
 #include "g2o/types/slam2d/edge_se2.h"
+#include "g2o/types/slam2d/edge_se2_offset.h"
 #include "g2o/types/slam2d/edge_se2_pointxy.h"
 #include "g2o/types/slam2d/edge_se2_pointxy_bearing.h"
+#include "g2o/types/slam2d/edge_se2_pointxy_offset.h"
 #include "g2o/types/slam2d/edge_se2_prior.h"
 #include "g2o/types/slam2d/edge_xy_prior.h"
+#include "g2o/types/slam2d/parameter_se2_offset.h"
 #include "unit_test/test_helper/typed_io.h"
 
-using Slam2DIoTypes = ::testing::Types<g2o::EdgeSE2, g2o::EdgeSE2PointXY,
-                                       g2o::EdgeSE2PointXYBearing,
-                                       g2o::EdgeSE2Prior, g2o::EdgeXYPrior>;
+using Slam2DIoTypes = ::testing::Types<
+    // without parameters
+    std::tuple<g2o::EdgeSE2>, std::tuple<g2o::EdgeSE2PointXY>,
+    std::tuple<g2o::EdgeSE2PointXYBearing>, std::tuple<g2o::EdgeSE2Prior>,
+    std::tuple<g2o::EdgeXYPrior>,
+    // with parameters
+    std::tuple<g2o::EdgeSE2Offset, g2o::ParameterSE2Offset,
+               g2o::ParameterSE2Offset>,
+    std::tuple<g2o::EdgeSE2PointXYOffset, g2o::ParameterSE2Offset> >;
 INSTANTIATE_TYPED_TEST_SUITE_P(Slam2D, FixedSizeEdgeIO, Slam2DIoTypes,
                                g2o::internal::DefaultTypeNames);

unit_test/slam3d/slam3d_io.cpp

@@ -26,10 +26,32 @@
 
 #include <gtest/gtest.h>
 
+#include <tuple>
+
 #include "g2o/types/slam3d/edge_pointxyz.h"
 #include "g2o/types/slam3d/edge_se3.h"
+#include "g2o/types/slam3d/edge_se3_offset.h"
+#include "g2o/types/slam3d/edge_se3_pointxyz.h"
+#include "g2o/types/slam3d/edge_se3_pointxyz_depth.h"
+#include "g2o/types/slam3d/edge_se3_pointxyz_disparity.h"
+#include "g2o/types/slam3d/edge_se3_prior.h"
+#include "g2o/types/slam3d/edge_se3_xyzprior.h"
+#include "g2o/types/slam3d/edge_xyz_prior.h"
+#include "g2o/types/slam3d/parameter_camera.h"
+#include "g2o/types/slam3d/parameter_se3_offset.h"
 #include "unit_test/test_helper/typed_io.h"
 
-using Slam3DIoTypes = ::testing::Types<g2o::EdgeSE3, g2o::EdgePointXYZ>;
+using Slam3DIoTypes = ::testing::Types<
+    // without parameters
+    std::tuple<g2o::EdgeSE3>, std::tuple<g2o::EdgePointXYZ>,
+    std::tuple<g2o::EdgeXYZPrior>,
+    // with parameters
+    std::tuple<g2o::EdgeSE3Offset, g2o::ParameterSE3Offset,
+               g2o::ParameterSE3Offset>,
+    std::tuple<g2o::EdgeSE3PointXYZDepth, g2o::ParameterCamera>,
+    std::tuple<g2o::EdgeSE3PointXYZDisparity, g2o::ParameterCamera>,
+    std::tuple<g2o::EdgeSE3PointXYZ, g2o::ParameterSE3Offset>,
+    std::tuple<g2o::EdgeSE3Prior, g2o::ParameterSE3Offset>,
+    std::tuple<g2o::EdgeSE3XYZPrior, g2o::ParameterSE3Offset> >;
 INSTANTIATE_TYPED_TEST_SUITE_P(Slam3D, FixedSizeEdgeIO, Slam3DIoTypes,
                                g2o::internal::DefaultTypeNames);

unit_test/test_helper/CMakeLists.txt

@@ -1,4 +1,5 @@
 add_library(unittest_helper ${G2O_LIB_TYPE}
+  utils.cpp
   allocate_optimizer.cpp allocate_optimizer.h
 )
-target_link_libraries(unittest_helper solver_eigen)
+target_link_libraries(unittest_helper solver_eigen stuff gmock)

unit_test/test_helper/random_state.h

@@ -24,6 +24,8 @@
 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+#pragma once
+
 #include <Eigen/Geometry>
 
 #include "g2o/core/eigen_types.h"

unit_test/test_helper/typed_io.h

@@ -24,33 +24,45 @@
 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+#pragma once
+
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
 #include <cstddef>
 #include <memory>
 #include <string>
+#include <tuple>
 
 #include "g2o/core/io/io_format.h"
 #include "g2o/core/optimizable_graph.h"
 #include "g2o/core/sparse_optimizer.h"
 #include "g2o/stuff/string_tools.h"
 #include "unit_test/test_helper/allocate_optimizer.h"
 #include "unit_test/test_helper/random_state.h"
-
+#include "unit_test/test_helper/utils.h"
+
+/**
+ * @brief A typed test for Edges.
+ *
+ * @tparam T A tuple like type whereas the first type gives the type of the Edge
+ * and the second determines the type of a parameter. Providing the type of a
+ * parameter is optional.
+ */
 template <typename T>
 struct FixedSizeEdgeIO : public ::testing::Test {
  public:
   FixedSizeEdgeIO() = default;
 
   void SetUp() override {
-    using EdgeType = T;
+    constexpr std::size_t kTupleSize = std::tuple_size_v<T>;
+    using EdgeType = typename std::tuple_element<0, T>::type;
 
     // 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) {
+    for (std::size_t i = 0; i < edge->vertices().size(); ++i) {
       auto v =
           std::shared_ptr<g2o::OptimizableGraph::Vertex>(edge->createVertex(i));
       edge->vertices()[i] = v;
@@ -60,6 +72,15 @@ struct FixedSizeEdgeIO : public ::testing::Test {
       this->optimizer_ptr_->addVertex(v);
     }
 
+    // Add the parameter types if required
+    if constexpr (kTupleSize > 1) {
+      using ParamTuple = typename TupleTypes<T>::Tail;
+      constexpr std::size_t kParamSize = std::tuple_size_v<ParamTuple>;
+      for (std::size_t i = 0; i < kParamSize; ++i) {
+        addNthParameter<kParamSize - 1, ParamTuple, EdgeType>(i, *edge);
+      }
+    }
+
     // Generate a random information matrix
     const typename EdgeType::InformationType information_matrix = []() {
       const typename EdgeType::InformationType aux =
@@ -80,8 +101,7 @@ struct FixedSizeEdgeIO : public ::testing::Test {
       g2o::internal::createOptimizerForTests();
 
   template <int I, typename EdgeType>
-  static std::enable_if_t<I == -1, void> initializeNthVertex(int /*i*/,
-                                                             EdgeType& /*t*/) {}
+  static std::enable_if_t<I == -1, void> initializeNthVertex(int, EdgeType&) {}
 
   template <int I, typename EdgeType>
   static std::enable_if_t<I != -1, void> initializeNthVertex(int i,
@@ -95,6 +115,31 @@ struct FixedSizeEdgeIO : public ::testing::Test {
     }
     initializeNthVertex<I - 1, EdgeType>(i, t);
   }
+
+  template <int I, typename ParameterTuple, typename EdgeType>
+  std::enable_if_t<I == -1, void> addNthParameter(int, EdgeType&) {}
+
+  template <int I, typename ParameterTuple, typename EdgeType>
+  std::enable_if_t<I != -1, void> addNthParameter(int i, EdgeType& edge) {
+    if (i != I) {
+      addNthParameter<I - 1, ParameterTuple, EdgeType>(i, edge);
+    }
+    using ParameterType = typename std::tuple_element<I, ParameterTuple>::type;
+    auto param = std::make_shared<ParameterType>();
+    param->setId(i + 100);
+    // TODO(Rainer): Modify the parameter to a random value
+    this->optimizer_ptr_->addParameter(param);
+    edge.setParameterId(i, param->id());
+  }
+
+  template <class TupleType>
+  struct TupleTypes;
+
+  template <class HeadType, class... TailTypes>
+  struct TupleTypes<std::tuple<HeadType, TailTypes...>> {
+    using Head = HeadType;
+    using Tail = std::tuple<TailTypes...>;
+  };
 };
 TYPED_TEST_SUITE_P(FixedSizeEdgeIO);
 
@@ -150,7 +195,7 @@ class DefaultTypeNames {
  public:
   template <typename T>
   static std::string GetName(int) {
-    return testing::internal::GetTypeName<T>();
+    return ExtractTupleHead(testing::internal::GetTypeName<T>());
   }
 };
 

unit_test/test_helper/utils.cpp

@@ -0,0 +1,45 @@
+// 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 "utils.h"
+
+#include <regex>
+
+#include "g2o/stuff/string_tools.h"
+
+namespace g2o::internal {
+std::string ExtractTupleHead(const std::string& input) {
+  const std::regex tuple_regex(".*tuple<([^,>]+)[,>].*");
+  std::smatch match;
+  if (!std::regex_match(input, match, tuple_regex)) return input;
+  if (match.size() != 2) return input;
+  std::string result = match[1].str();
+  if (strStartsWith(result, "g2o::")) {
+    return result.substr(5);
+  }
+  return result;
+}
+}  // namespace g2o::internal

unit_test/test_helper/utils.h

@@ -0,0 +1,35 @@
+// 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.
+
+#pragma once
+
+#include <string>
+
+namespace g2o::internal {
+
+std::string ExtractTupleHead(const std::string& input);
+
+}  // namespace g2o::internal