Local parameterization header & tests

sophus/ceres_local_parameterization.hpp

@@ -0,0 +1,101 @@
+#ifndef SOPHUS_CERES_LOCAL_PARAMETERIZATION_HPP
+#define SOPHUS_CERES_LOCAL_PARAMETERIZATION_HPP
+
+#include <ceres/local_parameterization.h>
+
+namespace Sophus {
+
+template <class T, std::size_t = sizeof(T)>
+constexpr std::true_type complete(T*);
+constexpr std::false_type complete(...);
+
+template <class T>
+using IsSpecialized = decltype(complete(std::declval<T*>()));
+
+/// 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
+///
+/// Primary use is mapping LieGroup::Tangent over raw data, with 2 options:
+///  - LieGroup::Tangent is "scalar" (for SO2), then we just dereference pointer
+///  - LieGroup::Tangent is Sophus::Vector<...>, then we need to use Eigen::Map
+///
+/// Specialization of Eigen::internal::traits<T> for T is crucial for
+/// for constructing Eigen::Map<T>, thus we use that property for distinguishing
+/// between those two options.
+/// At this moment there seem to be no option to check this using only
+/// "external" API of Eigen
+template <class T>
+using IsMappable = IsSpecialized<Eigen::internal::traits<std::decay_t<T>>>;
+
+template <class T>
+constexpr bool IsMappableV = IsMappable<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 ConstMap = const Scalar&;
+
+  static Map map(Scalar* ptr) noexcept { return *ptr; }
+  static ConstMap map(const Scalar* ptr) noexcept { return *ptr; }
+};
+
+template <typename T>
+struct Mapper<T, typename std::enable_if<IsMappableV<T>>::type> {
+  using Scalar = typename T::Scalar;
+  using Map = Eigen::Map<T>;
+  using ConstMap = Eigen::Map<const T>;
+
+  static Map map(Scalar* ptr) noexcept { return Map(ptr); }
+  static ConstMap map(const Scalar* ptr) noexcept { return ConstMap(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>::ConstMap;
+  static int constexpr DoF = LieGroupd::DoF;
+  static int constexpr num_parameters = LieGroupd::num_parameters;
+
+  /// LieGroup plus operation for Ceres
+  ///
+  ///  T * exp(x)
+  ///
+  bool Plus(double const* T_raw, double const* delta_raw,
+            double* T_plus_delta_raw) const override {
+    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
+  ///
+  bool ComputeJacobian(double const* T_raw,
+                       double* jacobian_raw) const override {
+    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;
+  }
+
+  int GlobalSize() const override { return LieGroupd::num_parameters; }
+
+  int LocalSize() const override { 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 test_ceres_so3)
+  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,44 @@
+#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;
+  so2_vec.push_back(RxSO2d::exp(Tangent(0.2, 1.)));
+  so2_vec.push_back(RxSO2d::exp(Tangent(0.2, 1.1)));
+  so2_vec.push_back(RxSO2d::exp(Tangent(0., 1.1)));
+  so2_vec.push_back(RxSO2d::exp(Tangent(0.00001, 0.)));
+  so2_vec.push_back(RxSO2d::exp(Tangent(0.00001, 0.00001)));
+  so2_vec.push_back(RxSO2d::exp(Tangent(kPi, 0.9)));
+  so2_vec.push_back(RxSO2d::exp(Tangent(0.2, 0)) *
+                    RxSO2d::exp(Tangent(kPi, 0.0)) *
+                    RxSO2d::exp(Tangent(-0.2, 0)));
+  so2_vec.push_back(RxSO2d::exp(Tangent(0.3, 0)) *
+                    RxSO2d::exp(Tangent(kPi, 0.001)) *
+                    RxSO2d::exp(Tangent(-0.3, 0)));
+
+  StdVector<Point> point_vec;
+  point_vec.push_back(Point(1.012, 2.73));
+  point_vec.push_back(Point(9.2, -7.3));
+  point_vec.push_back(Point(2.5, 0.1));
+  point_vec.push_back(Point(12.3, 1.9));
+  point_vec.push_back(Point(-3.21, 3.42));
+  point_vec.push_back(Point(-8.0, 6.1));
+  point_vec.push_back(Point(0.0, 2.5));
+  point_vec.push_back(Point(7.1, 7.8));
+  point_vec.push_back(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,48 @@
+#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;
+  rxso3_vec.push_back(RxSO3d::exp(Tangent(0.2, 0.5, 0.0, 1.)));
+  rxso3_vec.push_back(RxSO3d::exp(Tangent(0.2, 0.5, -1.0, 1.1)));
+  rxso3_vec.push_back(RxSO3d::exp(Tangent(0., 0., 0., 1.1)));
+  rxso3_vec.push_back(RxSO3d::exp(Tangent(0., 0., 0.00001, 0.)));
+  rxso3_vec.push_back(RxSO3d::exp(Tangent(0., 0., 0.00001, 0.00001)));
+  rxso3_vec.push_back(RxSO3d::exp(Tangent(0., 0., 0.00001, 0)));
+
+  rxso3_vec.push_back(RxSO3d::exp(Tangent(kPi, 0, 0, 0.9)));
+
+  rxso3_vec.push_back(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)));
+
+  rxso3_vec.push_back(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_vec.push_back(Point(1.012, 2.73, -1.4));
+  point_vec.push_back(Point(9.2, -7.3, -4.4));
+  point_vec.push_back(Point(2.5, 0.1, 9.1));
+  point_vec.push_back(Point(12.3, 1.9, 3.8));
+  point_vec.push_back(Point(-3.21, 3.42, 2.3));
+  point_vec.push_back(Point(-8.0, 6.1, -1.1));
+  point_vec.push_back(Point(0.0, 2.5, 5.9));
+  point_vec.push_back(Point(7.1, 7.8, -14));
+  point_vec.push_back(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,43 @@
+#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;
+  se2_vec.push_back(SE2d(SO2d(0.0), Point(0, 0)));
+  se2_vec.push_back(SE2d(SO2d(0.2), Point(10, 0)));
+  se2_vec.push_back(SE2d(SO2d(0.), Point(0, 100)));
+  se2_vec.push_back(SE2d(SO2d(-1.), Point(20, -1)));
+  se2_vec.push_back(SE2d(SO2d(0.00001), Point(-0.00000001, 0.0000000001)));
+  se2_vec.push_back(SE2d(SO2d(0.2), Point(0, 0)) *
+                    SE2d(SO2d(kPi), Point(0, 0)) *
+                    SE2d(SO2d(-0.2), Point(0, 0)));
+  se2_vec.push_back(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_vec.push_back(Point(1.012, 2.73));
+  point_vec.push_back(Point(9.2, -7.3));
+  point_vec.push_back(Point(2.5, 0.1));
+  point_vec.push_back(Point(12.3, 1.9));
+  point_vec.push_back(Point(-3.21, 3.42));
+  point_vec.push_back(Point(-8.0, 6.1));
+  point_vec.push_back(Point(0.0, 2.5));
+  point_vec.push_back(Point(7.1, 7.8));
+  point_vec.push_back(Point(5.8, 9.2));
+
+  std::cerr << "Test Ceres SE2" << std::endl;
+  Sophus::LieGroupCeresTests<Sophus::SE2>(se2_vec, point_vec).testAll();
+  return 0;
+}