Local parameterization header & tests

sophus/ceres_local_parameterization.hpp

@@ -0,0 +1,96 @@
+#ifndef SOPHUS_CERES_LOCAL_PARAMETERIZATION_HPP
+#define SOPHUS_CERES_LOCAL_PARAMETERIZATION_HPP
+
+namespace Sophus {
+
+/// Type trait used to distinguish mappable vector types from scalars
+/// We use this class to distinguish Sophus::Vector<Scalar, N> from Scalar types
+/// in LieGroup<T>::Tangent
+///
+/// Fortunately, ceres::Jet is not mappable
+template <typename T>
+struct is_mappable_type_t {
+  template <typename U>
+  using EigenTraits = Eigen::internal::traits<U>;
+  // Eigen::Map<T> requires Eigen::internal::traits<T> type to be complete
+  template <typename U>
+  static auto map_test(U*)
+      -> std::integral_constant<bool, sizeof(EigenTraits<U>) ==
+                                          sizeof(EigenTraits<U>)>;
+  static auto map_test(...) -> std::false_type;
+
+  using type = decltype(map_test((T*)nullptr));
+  static constexpr bool value = type::value;
+};
+
+template <typename T>
+constexpr bool is_mappable_type_v = is_mappable_type_t<T>::value;
+
+/// Helper for mapping tangent vectors (scalars) over pointers to data
+template <typename T, typename E = void>
+struct Mapper {
+  using Scalar = T;
+  using Map = Scalar&;
+  using CMap = const Scalar&;
+
+  static Map map(Scalar* ptr) { return *ptr; }
+  static CMap map(const Scalar* ptr) { return *ptr; }
+};
+
+template <typename T>
+struct Mapper<T, typename std::enable_if<is_mappable_type_v<T>>::type> {
+  using Scalar = typename T::Scalar;
+  using Map = Eigen::Map<T>;
+  using CMap = Eigen::Map<const T>;
+
+  static Map map(Scalar* ptr) { return Map(ptr); }
+  static CMap map(const Scalar* ptr) { return CMap(ptr); }
+};
+
+/// Templated local parameterization for LieGroup [with implemented
+/// LieGroup::Dx_this_mul_exp_x_at_0() ]
+template <template <typename, int = 0> class LieGroup>
+class LocalParameterization : public ceres::LocalParameterization {
+ public:
+  using LieGroupd = LieGroup<double>;
+  using Tangent = typename LieGroupd::Tangent;
+  using TangentMap = typename Sophus::Mapper<Tangent>::CMap;
+  static int constexpr DoF = LieGroupd::DoF;
+  static int constexpr num_parameters = LieGroupd::num_parameters;
+  virtual ~LocalParameterization() {}
+
+  /// LieGroup plus operation for Ceres
+  ///
+  ///  T * exp(x)
+  ///
+  virtual bool Plus(double const* T_raw, double const* delta_raw,
+                    double* T_plus_delta_raw) const {
+    Eigen::Map<LieGroupd const> const T(T_raw);
+    TangentMap delta = Sophus::Mapper<Tangent>::map(delta_raw);
+    Eigen::Map<LieGroupd> T_plus_delta(T_plus_delta_raw);
+    T_plus_delta = T * LieGroupd::exp(delta);
+    return true;
+  }
+
+  /// Jacobian of LieGroup plus operation for Ceres
+  ///
+  /// Dx T * exp(x)  with  x=0
+  ///
+  virtual bool ComputeJacobian(double const* T_raw,
+                               double* jacobian_raw) const {
+    Eigen::Map<LieGroupd const> T(T_raw);
+    Eigen::Map<Eigen::Matrix<double, num_parameters, DoF,
+                             DoF == 1 ? Eigen::ColMajor : Eigen::RowMajor>>
+        jacobian(jacobian_raw);
+    jacobian = T.Dx_this_mul_exp_x_at_0();
+    return true;
+  }
+
+  virtual int GlobalSize() const { return LieGroupd::num_parameters; }
+
+  virtual int LocalSize() const { return LieGroupd::DoF; }
+};
+
+}  // namespace Sophus
+
+#endif

test/ceres/CMakeLists.txt

@@ -8,7 +8,7 @@ if( Ceres_FOUND )
   MESSAGE(STATUS "CERES found")
 
   # Tests to run
-  SET( TEST_SOURCES test_ceres_se3 )
+  SET( TEST_SOURCES test_ceres_se3 test_ceres_so3 test_ceres_sim3 test_ceres_rxso3 test_ceres_se2 test_ceres_so2 test_ceres_rxso2 test_ceres_sim2 )
 
   FOREACH(test_src ${TEST_SOURCES})
     ADD_EXECUTABLE( ${test_src} ${test_src}.cpp local_parameterization_se3.hpp)

test/ceres/test_ceres_rxso2.cpp

@@ -0,0 +1,36 @@
+#include <ceres/ceres.h>
+#include <iostream>
+#include <sophus/rxso2.hpp>
+
+#include "tests.hpp"
+
+template <typename T>
+using StdVector = std::vector<T, Eigen::aligned_allocator<T>>;
+
+int main(int, char**) {
+  using RxSO2d = Sophus::RxSO2d;
+  using Tangent = RxSO2d::Tangent;
+  using Point = RxSO2d::Point;
+  double const kPi = Sophus::Constants<double>::pi();
+
+  StdVector<RxSO2d> so2_vec = {
+      RxSO2d::exp(Tangent(0.2, 1.)),
+      RxSO2d::exp(Tangent(0.2, 1.1)),
+      RxSO2d::exp(Tangent(0., 1.1)),
+      RxSO2d::exp(Tangent(0.00001, 0.)),
+      RxSO2d::exp(Tangent(0.00001, 0.00001)),
+      RxSO2d::exp(Tangent(kPi, 0.9)),
+      RxSO2d::exp(Tangent(0.2, 0)) * RxSO2d::exp(Tangent(kPi, 0.0)) *
+          RxSO2d::exp(Tangent(-0.2, 0)),
+      RxSO2d::exp(Tangent(0.3, 0)) * RxSO2d::exp(Tangent(kPi, 0.001)) *
+          RxSO2d::exp(Tangent(-0.3, 0))};
+
+  StdVector<Point> point_vec = {
+      Point(1.012, 2.73), Point(9.2, -7.3),   Point(2.5, 0.1),
+      Point(12.3, 1.9),   Point(-3.21, 3.42), Point(-8.0, 6.1),
+      Point(0.0, 2.5),    Point(7.1, 7.8),    Point(5.8, 9.2)};
+
+  std::cerr << "Test Ceres RxSO2" << std::endl;
+  Sophus::LieGroupCeresTests<Sophus::RxSO2>(so2_vec, point_vec).testAll();
+  return 0;
+}

test/ceres/test_ceres_rxso3.cpp

@@ -0,0 +1,41 @@
+#include <ceres/ceres.h>
+#include <iostream>
+#include <sophus/rxso3.hpp>
+
+#include "tests.hpp"
+
+template <typename T>
+using StdVector = std::vector<T, Eigen::aligned_allocator<T>>;
+
+int main(int, char**) {
+  using RxSO3d = Sophus::RxSO3d;
+  using Point = RxSO3d::Point;
+  using Tangent = RxSO3d::Tangent;
+  double const kPi = Sophus::Constants<double>::pi();
+
+  StdVector<RxSO3d> rxso3_vec = {RxSO3d::exp(Tangent(0.2, 0.5, 0.0, 1.)),
+                                 RxSO3d::exp(Tangent(0.2, 0.5, -1.0, 1.1)),
+                                 RxSO3d::exp(Tangent(0., 0., 0., 1.1)),
+                                 RxSO3d::exp(Tangent(0., 0., 0.00001, 0.)),
+                                 RxSO3d::exp(Tangent(0., 0., 0.00001, 0.00001)),
+                                 RxSO3d::exp(Tangent(0., 0., 0.00001, 0)),
+
+                                 RxSO3d::exp(Tangent(kPi, 0, 0, 0.9)),
+
+                                 RxSO3d::exp(Tangent(0.2, -0.5, 0, 0)) *
+                                     RxSO3d::exp(Tangent(kPi, 0, 0, 0)) *
+                                     RxSO3d::exp(Tangent(-0.2, -0.5, 0, 0)),
+
+                                 RxSO3d::exp(Tangent(0.3, 0.5, 0.1, 0)) *
+                                     RxSO3d::exp(Tangent(kPi, 0, 0, 0)) *
+                                     RxSO3d::exp(Tangent(-0.3, -0.5, -0.1, 0))};
+
+  StdVector<Point> point_vec = {
+      Point(1.012, 2.73, -1.4), Point(9.2, -7.3, -4.4),  Point(2.5, 0.1, 9.1),
+      Point(12.3, 1.9, 3.8),    Point(-3.21, 3.42, 2.3), Point(-8.0, 6.1, -1.1),
+      Point(0.0, 2.5, 5.9),     Point(7.1, 7.8, -14),    Point(5.8, 9.2, 0.0)};
+
+  std::cerr << "Test Ceres RxSO2" << std::endl;
+  Sophus::LieGroupCeresTests<Sophus::RxSO3>(rxso3_vec, point_vec).testAll();
+  return 0;
+}

test/ceres/test_ceres_se2.cpp

@@ -0,0 +1,35 @@
+#include <ceres/ceres.h>
+#include <iostream>
+#include <sophus/se2.hpp>
+
+#include "tests.hpp"
+
+template <typename T>
+using StdVector = std::vector<T, Eigen::aligned_allocator<T>>;
+
+int main(int, char**) {
+  using SE2d = Sophus::SE2d;
+  using SO2d = Sophus::SO2d;
+  using Point = SE2d::Point;
+  double const kPi = Sophus::Constants<double>::pi();
+
+  StdVector<SE2d> se2_vec = {
+      SE2d(SO2d(0.0), Point(0, 0)),
+      SE2d(SO2d(0.2), Point(10, 0)),
+      SE2d(SO2d(0.), Point(0, 100)),
+      SE2d(SO2d(-1.), Point(20, -1)),
+      SE2d(SO2d(0.00001), Point(-0.00000001, 0.0000000001)),
+      SE2d(SO2d(0.2), Point(0, 0)) * SE2d(SO2d(kPi), Point(0, 0)) *
+          SE2d(SO2d(-0.2), Point(0, 0)),
+      SE2d(SO2d(0.3), Point(2, 0)) * SE2d(SO2d(kPi), Point(0, 0)) *
+          SE2d(SO2d(-0.3), Point(0, 6))};
+
+  StdVector<Point> point_vec = {
+      Point(1.012, 2.73), Point(9.2, -7.3),   Point(2.5, 0.1),
+      Point(12.3, 1.9),   Point(-3.21, 3.42), Point(-8.0, 6.1),
+      Point(0.0, 2.5),    Point(7.1, 7.8),    Point(5.8, 9.2)};
+
+  std::cerr << "Test Ceres SE2" << std::endl;
+  Sophus::LieGroupCeresTests<Sophus::SE2>(se2_vec, point_vec).testAll();
+  return 0;
+}