From 2063e5e4c507ad517d6c2eb228b45d601c3e3207 Mon Sep 17 00:00:00 2001
From: Dmitriy Korchemkin <dmitriy.korchemkin@gmail.com>
Date: Fri, 5 Feb 2021 22:32:07 +0300
Subject: [PATCH] Local parameterization header & tests

---
 sophus/ceres_local_parameterization.hpp | 101 ++++++++
 test/ceres/CMakeLists.txt               |   2 +-
 test/ceres/test_ceres_rxso2.cpp         |  44 ++++
 test/ceres/test_ceres_rxso3.cpp         |  48 ++++
 test/ceres/test_ceres_se2.cpp           |  43 ++++
 test/ceres/test_ceres_se3.cpp           | 216 +++--------------
 test/ceres/test_ceres_sim2.cpp          |  56 +++++
 test/ceres/test_ceres_sim3.cpp          |  57 +++++
 test/ceres/test_ceres_so2.cpp           |  31 +++
 test/ceres/test_ceres_so3.cpp           | 117 ++++-----
 test/ceres/tests.hpp                    | 306 ++++++++++++++++++++++++
 11 files changed, 762 insertions(+), 259 deletions(-)
 create mode 100644 sophus/ceres_local_parameterization.hpp
 create mode 100644 test/ceres/test_ceres_rxso2.cpp
 create mode 100644 test/ceres/test_ceres_rxso3.cpp
 create mode 100644 test/ceres/test_ceres_se2.cpp
 create mode 100644 test/ceres/test_ceres_sim2.cpp
 create mode 100644 test/ceres/test_ceres_sim3.cpp
 create mode 100644 test/ceres/test_ceres_so2.cpp
 create mode 100644 test/ceres/tests.hpp

diff --git a/sophus/ceres_local_parameterization.hpp b/sophus/ceres_local_parameterization.hpp
new file mode 100644
index 000000000..78e583b73
--- /dev/null
+++ b/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
diff --git a/test/ceres/CMakeLists.txt b/test/ceres/CMakeLists.txt
index 402aa4104..6d076e114 100644
--- a/test/ceres/CMakeLists.txt
+++ b/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)
diff --git a/test/ceres/test_ceres_rxso2.cpp b/test/ceres/test_ceres_rxso2.cpp
new file mode 100644
index 000000000..6bd7a0d8c
--- /dev/null
+++ b/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;
+}
diff --git a/test/ceres/test_ceres_rxso3.cpp b/test/ceres/test_ceres_rxso3.cpp
new file mode 100644
index 000000000..1a1a24095
--- /dev/null
+++ b/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;
+}
diff --git a/test/ceres/test_ceres_se2.cpp b/test/ceres/test_ceres_se2.cpp
new file mode 100644
index 000000000..baf0642ef
--- /dev/null
+++ b/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;
+}
diff --git a/test/ceres/test_ceres_se3.cpp b/test/ceres/test_ceres_se3.cpp
index 00650b287..57b2ace56 100644
--- a/test/ceres/test_ceres_se3.cpp
+++ b/test/ceres/test_ceres_se3.cpp
@@ -2,191 +2,45 @@
 #include <iostream>
 #include <sophus/se3.hpp>
 
-#include "local_parameterization_se3.hpp"
+#include "tests.hpp"
 
-// Eigen's ostream operator is not compatible with ceres::Jet types.
-// In particular, Eigen assumes that the scalar type (here Jet<T,N>) can be
-// casted to an arithmetic type, which is not true for ceres::Jet.
-// Unfortunately, the ceres::Jet class does not define a conversion
-// operator (http://en.cppreference.com/w/cpp/language/cast_operator).
-//
-// This workaround creates a template specialization for Eigen's cast_impl,
-// when casting from a ceres::Jet type. It relies on Eigen's internal API and
-// might break with future versions of Eigen.
-namespace Eigen {
-namespace internal {
-
-template <class T, int N, typename NewType>
-struct cast_impl<ceres::Jet<T, N>, NewType> {
-  EIGEN_DEVICE_FUNC
-  static inline NewType run(ceres::Jet<T, N> const& x) {
-    return static_cast<NewType>(x.a);
-  }
-};
-
-}  // namespace internal
-}  // namespace Eigen
-
-struct TestSE3CostFunctor {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  TestSE3CostFunctor(Sophus::SE3d T_aw) : T_aw(T_aw) {}
-
-  template <class T>
-  bool operator()(T const* const sT_wa, T* sResiduals) const {
-    Eigen::Map<Sophus::SE3<T> const> const T_wa(sT_wa);
-    Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(sResiduals);
-
-    // We are able to mix Sophus types with doubles and Jet types without
-    // needing to cast to T.
-    residuals = (T_aw * T_wa).log();
-    // Reverse order of multiplication. This forces the compiler to verify that
-    // (Jet, double) and (double, Jet) SE3 multiplication work correctly.
-    residuals = (T_wa * T_aw).log();
-    // Finally, ensure that Jet-to-Jet multiplication works.
-    residuals = (T_wa * T_aw.cast<T>()).log();
-    return true;
-  }
-
-  Sophus::SE3d T_aw;
-};
-
-struct TestPointCostFunctor {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  TestPointCostFunctor(Sophus::SE3d T_aw, Eigen::Vector3d point_a)
-      : T_aw(T_aw), point_a(point_a) {}
-
-  template <class T>
-  bool operator()(T const* const sT_wa, T const* const spoint_b,
-                  T* sResiduals) const {
-    using Vector3T = Eigen::Matrix<T, 3, 1>;
-    Eigen::Map<Sophus::SE3<T> const> const T_wa(sT_wa);
-    Eigen::Map<Vector3T const> point_b(spoint_b);
-    Eigen::Map<Vector3T> residuals(sResiduals);
-
-    // Multiply SE3d by Jet Vector3.
-    Vector3T point_b_prime = T_aw * point_b;
-    // Ensure Jet SE3 multiplication with Jet Vector3.
-    point_b_prime = T_aw.cast<T>() * point_b;
-
-    // Multiply Jet SE3 with Vector3d.
-    Vector3T point_a_prime = T_wa * point_a;
-    // Ensure Jet SE3 multiplication with Jet Vector3.
-    point_a_prime = T_wa * point_a.cast<T>();
-
-    residuals = point_b_prime - point_a_prime;
-    return true;
-  }
-
-  Sophus::SE3d T_aw;
-  Eigen::Vector3d point_a;
-};
-
-bool test(Sophus::SE3d const& T_w_targ, Sophus::SE3d const& T_w_init,
-          Sophus::SE3d::Point const& point_a_init,
-          Sophus::SE3d::Point const& point_b) {
-  static constexpr int kNumPointParameters = 3;
-
-  // Optimisation parameters.
-  Sophus::SE3d T_wr = T_w_init;
-  Sophus::SE3d::Point point_a = point_a_init;
-
-  // Build the problem.
-  ceres::Problem problem;
-
-  // Specify local update rule for our parameter
-  problem.AddParameterBlock(T_wr.data(), Sophus::SE3d::num_parameters,
-                            new Sophus::test::LocalParameterizationSE3);
-
-  // Create and add cost functions. Derivatives will be evaluated via
-  // automatic differentiation
-  ceres::CostFunction* cost_function1 =
-      new ceres::AutoDiffCostFunction<TestSE3CostFunctor, Sophus::SE3d::DoF,
-                                      Sophus::SE3d::num_parameters>(
-          new TestSE3CostFunctor(T_w_targ.inverse()));
-  problem.AddResidualBlock(cost_function1, NULL, T_wr.data());
-  ceres::CostFunction* cost_function2 =
-      new ceres::AutoDiffCostFunction<TestPointCostFunctor, kNumPointParameters,
-                                      Sophus::SE3d::num_parameters,
-                                      kNumPointParameters>(
-          new TestPointCostFunctor(T_w_targ.inverse(), point_b));
-  problem.AddResidualBlock(cost_function2, NULL, T_wr.data(), point_a.data());
-
-  // Set solver options (precision / method)
-  ceres::Solver::Options options;
-  options.gradient_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
-  options.function_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
-  options.linear_solver_type = ceres::DENSE_QR;
-
-  // Solve
-  ceres::Solver::Summary summary;
-  Solve(options, &problem, &summary);
-  std::cout << summary.BriefReport() << std::endl;
-
-  // Difference between target and parameter
-  double const mse = (T_w_targ.inverse() * T_wr).log().squaredNorm();
-  bool const passed = mse < 10. * Sophus::Constants<double>::epsilon();
-  return passed;
-}
-
-template <typename Scalar>
-bool CreateSE3FromMatrix(const Eigen::Matrix<Scalar, 4, 4>& mat) {
-  Sophus::SE3<Scalar> se3 = Sophus::SE3<Scalar>(mat);
-  std::cout << se3.translation().x() << std::endl;
-  return true;
-}
+template <typename T>
+using StdVector = std::vector<T, Eigen::aligned_allocator<T>>;
 
 int main(int, char**) {
-  using SE3Type = Sophus::SE3<double>;
-  using SO3Type = Sophus::SO3<double>;
-  using Point = SE3Type::Point;
+  using SE3d = Sophus::SE3d;
+  using SO3d = Sophus::SO3d;
+  using Point = SE3d::Point;
   double const kPi = Sophus::Constants<double>::pi();
 
-  std::vector<SE3Type> se3_vec;
-  se3_vec.push_back(
-      SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)));
-  se3_vec.push_back(
-      SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)), Point(10, 0, 0)));
-  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)), Point(0, 100, 5)));
-  se3_vec.push_back(
-      SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0, 0, 0)));
-  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)),
-                            Point(0, -0.00000001, 0.0000000001)));
-  se3_vec.push_back(
-      SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0.01, 0, 0)));
-  se3_vec.push_back(SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(4, -5, 0)));
-  se3_vec.push_back(
-      SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)) *
-      SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) *
-      SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)), Point(0, 0, 0)));
-  se3_vec.push_back(
-      SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)), Point(2, 0, -7)) *
-      SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) *
-      SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)), Point(0, 6, 0)));
-
-  std::vector<Point> point_vec;
-  point_vec.emplace_back(1.012, 2.73, -1.4);
-  point_vec.emplace_back(9.2, -7.3, -4.4);
-  point_vec.emplace_back(2.5, 0.1, 9.1);
-  point_vec.emplace_back(12.3, 1.9, 3.8);
-  point_vec.emplace_back(-3.21, 3.42, 2.3);
-  point_vec.emplace_back(-8.0, 6.1, -1.1);
-  point_vec.emplace_back(0.0, 2.5, 5.9);
-  point_vec.emplace_back(7.1, 7.8, -14);
-  point_vec.emplace_back(5.8, 9.2, 0.0);
-
-  for (size_t i = 0; i < se3_vec.size(); ++i) {
-    const int other_index = (i + 3) % se3_vec.size();
-    bool const passed = test(se3_vec[i], se3_vec[other_index], point_vec[i],
-                             point_vec[other_index]);
-    if (!passed) {
-      std::cerr << "failed!" << std::endl << std::endl;
-      exit(-1);
-    }
-  }
-
-  Eigen::Matrix<ceres::Jet<double, 28>, 4, 4> mat;
-  mat.setIdentity();
-  std::cout << CreateSE3FromMatrix(mat) << std::endl;
-
+  StdVector<SE3d> se3_vec;
+  se3_vec.push_back(SE3d(SO3d::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)));
+  se3_vec.push_back(SE3d(SO3d::exp(Point(0.2, 0.5, -1.0)), Point(10, 0, 0)));
+  se3_vec.push_back(SE3d(SO3d::exp(Point(0., 0., 0.)), Point(0, 100, 5)));
+  se3_vec.push_back(SE3d(SO3d::exp(Point(0., 0., 0.00001)), Point(0, 0, 0)));
+  se3_vec.push_back(SE3d(SO3d::exp(Point(0., 0., 0.00001)),
+                         Point(0, -0.00000001, 0.0000000001)));
+  se3_vec.push_back(SE3d(SO3d::exp(Point(0., 0., 0.00001)), Point(0.01, 0, 0)));
+  se3_vec.push_back(SE3d(SO3d::exp(Point(kPi, 0, 0)), Point(4, -5, 0)));
+  se3_vec.push_back(SE3d(SO3d::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)) *
+                    SE3d(SO3d::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) *
+                    SE3d(SO3d::exp(Point(-0.2, -0.5, -0.0)), Point(0, 0, 0)));
+  se3_vec.push_back(SE3d(SO3d::exp(Point(0.3, 0.5, 0.1)), Point(2, 0, -7)) *
+                    SE3d(SO3d::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) *
+                    SE3d(SO3d::exp(Point(-0.3, -0.5, -0.1)), Point(0, 6, 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 SE3" << std::endl;
+  Sophus::LieGroupCeresTests<Sophus::SE3>(se3_vec, point_vec).testAll();
   return 0;
 }
diff --git a/test/ceres/test_ceres_sim2.cpp b/test/ceres/test_ceres_sim2.cpp
new file mode 100644
index 000000000..e02740e6f
--- /dev/null
+++ b/test/ceres/test_ceres_sim2.cpp
@@ -0,0 +1,56 @@
+#include <ceres/ceres.h>
+#include <iostream>
+#include <sophus/sim2.hpp>
+
+#include "tests.hpp"
+
+template <typename T>
+using StdVector = std::vector<T, Eigen::aligned_allocator<T>>;
+
+int main(int, char**) {
+  using Sim2d = Sophus::Sim2d;
+  using RxSO2d = Sophus::RxSO2d;
+  using Point = Sim2d::Point;
+  using Vector2d = Sophus::Vector2d;
+  double const kPi = Sophus::Constants<double>::pi();
+
+  StdVector<Sim2d> sim2_vec;
+
+  sim2_vec.push_back(Sim2d(RxSO2d::exp(Vector2d(0.2, 1.)), Point(0, 0)));
+
+  sim2_vec.push_back(Sim2d(RxSO2d::exp(Vector2d(0.2, 1.1)), Point(10, 0)));
+
+  sim2_vec.push_back(Sim2d(RxSO2d::exp(Vector2d(0., 0.)), Point(0, 10)));
+
+  sim2_vec.push_back(Sim2d(RxSO2d::exp(Vector2d(0.00001, 0.)), Point(0, 0)));
+
+  sim2_vec.push_back(
+      Sim2d(RxSO2d::exp(Vector2d(0.00001, 0.0000001)), Point(1, -1.00000001)));
+
+  sim2_vec.push_back(Sim2d(RxSO2d::exp(Vector2d(0., 0.)), Point(0.01, 0)));
+
+  sim2_vec.push_back(Sim2d(RxSO2d::exp(Vector2d(kPi, 0.9)), Point(4, 0)));
+
+  sim2_vec.push_back(Sim2d(RxSO2d::exp(Vector2d(0.2, 0)), Point(0, 0)) *
+                     Sim2d(RxSO2d::exp(Vector2d(kPi, 0)), Point(0, 0)) *
+                     Sim2d(RxSO2d::exp(Vector2d(-0.2, 0)), Point(0, 0)));
+
+  sim2_vec.push_back(Sim2d(RxSO2d::exp(Vector2d(0.3, 0)), Point(2, -7)) *
+                     Sim2d(RxSO2d::exp(Vector2d(kPi, 0)), Point(0, 0)) *
+                     Sim2d(RxSO2d::exp(Vector2d(-0.3, 0)), 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 Sim2" << std::endl;
+  Sophus::LieGroupCeresTests<Sophus::Sim2>(sim2_vec, point_vec).testAll();
+  return 0;
+}
diff --git a/test/ceres/test_ceres_sim3.cpp b/test/ceres/test_ceres_sim3.cpp
new file mode 100644
index 000000000..fd404c07c
--- /dev/null
+++ b/test/ceres/test_ceres_sim3.cpp
@@ -0,0 +1,57 @@
+#include <ceres/ceres.h>
+#include <iostream>
+#include <sophus/sim3.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 Sim3d = Sophus::Sim3d;
+  using Point = Sim3d::Point;
+  using Vector4d = Eigen::Vector4d;
+  double const kPi = Sophus::Constants<double>::pi();
+
+  StdVector<Sim3d> sim3_vec;
+  sim3_vec.push_back(
+      Sim3d(RxSO3d::exp(Vector4d(0.2, 0.5, 0.0, 1.)), Point(0, 0, 0)));
+  sim3_vec.push_back(
+      Sim3d(RxSO3d::exp(Vector4d(0.2, 0.5, -1.0, 1.1)), Point(10, 0, 0)));
+  sim3_vec.push_back(
+      Sim3d(RxSO3d::exp(Vector4d(0., 0., 0., 0.001)), Point(0, 10, 5)));
+  sim3_vec.push_back(
+      Sim3d(RxSO3d::exp(Vector4d(0., 0., 0., 1.1)), Point(0, 10, 5)));
+  sim3_vec.push_back(
+      Sim3d(RxSO3d::exp(Vector4d(0., 0., 0.00001, 0.)), Point(0, 0, 0)));
+  sim3_vec.push_back(Sim3d(RxSO3d::exp(Vector4d(0., 0., 0.00001, 0.0000001)),
+                           Point(1, -1.00000001, 2.0000000001)));
+  sim3_vec.push_back(
+      Sim3d(RxSO3d::exp(Vector4d(0., 0., 0.00001, 0)), Point(0.01, 0, 0)));
+  sim3_vec.push_back(
+      Sim3d(RxSO3d::exp(Vector4d(kPi, 0, 0, 0.9)), Point(4, -5, 0)));
+  sim3_vec.push_back(
+      Sim3d(RxSO3d::exp(Vector4d(0.2, 0.5, 0.0, 0)), Point(0, 0, 0)) *
+      Sim3d(RxSO3d::exp(Vector4d(kPi, 0, 0, 0)), Point(0, 0, 0)) *
+      Sim3d(RxSO3d::exp(Vector4d(-0.2, -0.5, -0.0, 0)), Point(0, 0, 0)));
+  sim3_vec.push_back(
+      Sim3d(RxSO3d::exp(Vector4d(0.3, 0.5, 0.1, 0)), Point(2, 0, -7)) *
+      Sim3d(RxSO3d::exp(Vector4d(kPi, 0, 0, 0)), Point(0, 0, 0)) *
+      Sim3d(RxSO3d::exp(Vector4d(-0.3, -0.5, -0.1, 0)), Point(0, 6, 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 Sim3" << std::endl;
+  Sophus::LieGroupCeresTests<Sophus::Sim3>(sim3_vec, point_vec).testAll();
+  return 0;
+}
diff --git a/test/ceres/test_ceres_so2.cpp b/test/ceres/test_ceres_so2.cpp
new file mode 100644
index 000000000..fab25ca10
--- /dev/null
+++ b/test/ceres/test_ceres_so2.cpp
@@ -0,0 +1,31 @@
+#include <ceres/ceres.h>
+#include <iostream>
+#include <sophus/so2.hpp>
+
+#include "tests.hpp"
+
+template <typename T>
+using StdVector = std::vector<T, Eigen::aligned_allocator<T>>;
+
+int main(int, char**) {
+  using SO2d = Sophus::SO2d;
+  using Point = SO2d::Point;
+  double const kPi = Sophus::Constants<double>::pi();
+
+  StdVector<SO2d> so2_vec = {SO2d::exp(0.0),
+                             SO2d::exp(0.2),
+                             SO2d::exp(10.),
+                             SO2d::exp(0.00001),
+                             SO2d::exp(kPi),
+                             SO2d::exp(0.2) * SO2d::exp(kPi) * SO2d::exp(-0.2),
+                             SO2d::exp(-0.3) * SO2d::exp(kPi) * SO2d::exp(0.3)};
+
+  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 SO2" << std::endl;
+  Sophus::LieGroupCeresTests<Sophus::SO2>(so2_vec, point_vec).testAll();
+  return 0;
+}
diff --git a/test/ceres/test_ceres_so3.cpp b/test/ceres/test_ceres_so3.cpp
index 030d9f53f..771ec5537 100644
--- a/test/ceres/test_ceres_so3.cpp
+++ b/test/ceres/test_ceres_so3.cpp
@@ -1,69 +1,15 @@
 #include <ceres/ceres.h>
 #include <iostream>
-#include <sophus/so3.hpp>
+#include <sophus/se3.hpp>
 
-struct TestCostFunctor {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  TestCostFunctor(Sophus::SO3d C_aw) : C_aw(C_aw) {}
+#include "tests.hpp"
 
-  template <class T>
-  bool operator()(T const* const sC_wa, T* sResiduals) const {
-    Eigen::Map<Sophus::SO3<T> const> const C_wa(sC_wa);
-    Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals(sResiduals);
-
-    // We are able to mix Sophus types with doubles and Jet types without
-    // needing to cast to T.
-    residuals = (C_aw * C_wa).log();
-    // Reverse order of multiplication. This forces the compiler to verify that
-    // (Jet, double) and (double, Jet) SO3 multiplication work correctly.
-    residuals = (C_wa * C_aw).log();
-    // Finally, ensure that Jet-to-Jet multiplication works.
-    residuals = (C_wa * C_aw.cast<T>()).log();
-    return true;
-  }
-
-  Sophus::SO3d C_aw;
-};
-
-// Checks if ceres optimization will proceed correctly given problematic or
-// close-to-singular initial conditions, i.e. approx. 180-deg rotation, which
-// trips a flaw in old implementation of SO3::log() where the tangent vector's
-// magnitude is set to a constant close to \pi whenever the input rotation's
-// rotation angle is within some tolerance of \pi, giving zero gradients wrt
-// scalar part of quaternion.
-bool test(Sophus::SO3d const& C_w_targ, Sophus::SO3d const& C_w_init) {
-  Sophus::SO3d C_wr = C_w_init;
-  ceres::Problem problem;
-  // Specify local update rule for our parameter.
-  problem.AddParameterBlock(C_wr.data(), Sophus::SO3d::num_parameters,
-                            new ceres::EigenQuaternionParameterization);
-
-  // Create and add cost functions. Derivatives will be evaluated via
-  // automatic differentiation
-  ceres::CostFunction* cosC_function =
-      new ceres::AutoDiffCostFunction<TestCostFunctor, Sophus::SO3d::DoF,
-                                      Sophus::SO3d::num_parameters>(
-          new TestCostFunctor(C_w_targ.inverse()));
-  problem.AddResidualBlock(cosC_function, NULL, C_wr.data());
-
-  // Set solver options (precision / method)
-  ceres::Solver::Options options;
-  options.gradient_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
-  options.function_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
-  options.linear_solver_type = ceres::DENSE_QR;
-
-  ceres::Solver::Summary summary;
-  Solve(options, &problem, &summary);
-  std::cout << summary.BriefReport() << std::endl;
-
-  // Difference between target and parameter
-  double const mse = (C_w_targ.inverse() * C_wr).log().squaredNorm();
-  bool const passed = mse < 10. * Sophus::Constants<double>::epsilon();
-  return passed;
-}
+template <typename T>
+using StdVector = std::vector<T, Eigen::aligned_allocator<T>>;
 
 int main(int, char**) {
   using SO3Type = Sophus::SO3<double>;
+  using Point = SO3Type::Point;
   using Tangent = SO3Type::Tangent;
   double const kPi = Sophus::Constants<double>::pi();
   double const epsilon = Sophus::Constants<double>::epsilon();
@@ -71,24 +17,41 @@ int main(int, char**) {
   SO3Type C_0 = SO3Type::exp(Tangent(0.1, 0.05, -0.7));
 
   Tangent axis_0(0.18005924, -0.54563405, 0.81845107);
-  auto ics = {C_0 * SO3Type::exp(axis_0 * 1.0),  // Generic rotation angle < pi
-              C_0 * SO3Type::exp(axis_0 * -1.0),
-              C_0 * SO3Type::exp(axis_0 * 4.0),  // Generic rotation angle > pi
-              C_0 * SO3Type::exp(axis_0 * -4.0),
-              C_0 * SO3Type::exp(axis_0 * kPi),  // Singular rotation angle = pi
-              C_0 * SO3Type::exp(axis_0 * -kPi),
-              C_0 * SO3Type::exp(axis_0 * kPi * (1.0 + epsilon)),
-              C_0 * SO3Type::exp(axis_0 * kPi * (1.0 - epsilon)),
-              C_0 * SO3Type::exp(axis_0 * -kPi * (1.0 + epsilon)),
-              C_0 * SO3Type::exp(axis_0 * -kPi * (1.0 - epsilon))};
-  // Now solve problems.
-  for (const auto& it : ics) {
-    bool const passed = test(C_0, it);
-    if (!passed) {
-      std::cerr << "failed!" << std::endl << std::endl;
-      exit(-1);
-    }
-  }
 
+  StdVector<SO3Type> so3_vec = {
+      // Generic tests
+      SO3Type::exp(Point(0.2, 0.5, 0.0)), SO3Type::exp(Point(0.2, 0.5, -1.0)),
+      SO3Type::exp(Point(0., 0., 0.)), SO3Type::exp(Point(0., 0., 0.00001)),
+      SO3Type::exp(Point(kPi, 0, 0)),
+      SO3Type::exp(Point(0.2, 0.5, 0.0)) * SO3Type::exp(Point(kPi, 0, 0)) *
+          SO3Type::exp(Point(-0.2, -0.5, -0.0)),
+      SO3Type::exp(Point(0.3, 0.5, 0.1)) * SO3Type::exp(Point(kPi, 0, 0)) *
+          SO3Type::exp(Point(-0.3, -0.5, -0.1)),
+
+      // Checks if ceres optimization will proceed correctly given problematic
+      // or close-to-singular initial conditions, i.e. approx. 180-deg rotation,
+      // which trips a flaw in old implementation of SO3::log() where the
+      // tangent vector's magnitude is set to a constant close to \pi whenever
+      // the input rotation's rotation angle is within some tolerance of \pi,
+      // giving zero gradients wrt scalar part of quaternion.
+      C_0,
+      C_0 * SO3Type::exp(axis_0 * 1.0),  // Generic rotation angle < pi
+      C_0 * SO3Type::exp(axis_0 * -1.0),
+      C_0 * SO3Type::exp(axis_0 * 4.0),  // Generic rotation angle > pi
+      C_0 * SO3Type::exp(axis_0 * -4.0),
+      C_0 * SO3Type::exp(axis_0 * kPi),  // Singular rotation angle = pi
+      C_0 * SO3Type::exp(axis_0 * -kPi),
+      C_0 * SO3Type::exp(axis_0 * kPi * (1.0 + epsilon)),
+      C_0 * SO3Type::exp(axis_0 * kPi * (1.0 - epsilon)),
+      C_0 * SO3Type::exp(axis_0 * -kPi * (1.0 + epsilon)),
+      C_0 * SO3Type::exp(axis_0 * -kPi * (1.0 - epsilon))};
+
+  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 SO3" << std::endl;
+  Sophus::LieGroupCeresTests<Sophus::SO3>(so3_vec, point_vec).testAll();
   return 0;
 }
diff --git a/test/ceres/tests.hpp b/test/ceres/tests.hpp
new file mode 100644
index 000000000..a993ec201
--- /dev/null
+++ b/test/ceres/tests.hpp
@@ -0,0 +1,306 @@
+#ifndef SOPHUS_CERES_TESTS_HPP
+#define SOPHUS_CERES_TESTS_HPP
+
+#include <ceres/ceres.h>
+
+#include <sophus/ceres_local_parameterization.hpp>
+#include <sophus/test_macros.hpp>
+
+namespace Sophus {
+
+template <int N>
+double dot(const Vector<double, N>& v1, const Vector<double, N>& v2) {
+  return v1.dot(v2);
+}
+
+double dot(const double& a, const double& b) { return a * b; }
+
+template <int N>
+double squaredNorm(const Vector<double, N>& vec) {
+  return vec.squaredNorm();
+}
+
+double squaredNorm(const double& scalar) { return scalar * scalar; }
+
+template <typename T>
+struct Random {
+  template <typename R>
+  T static sample(R& rng) {
+    std::normal_distribution<double> rnorm;
+    static_assert(T::RowsAtCompileTime >= 0,
+                  "Matrix should have known size at compile-time");
+    static_assert(T::ColsAtCompileTime >= 0,
+                  "Matrix should have known size at compile-time");
+    T res;
+    for (Eigen::Index i = 0; i < res.size(); ++i) res.data()[i] = rnorm(rng);
+    return res;
+  }
+};
+
+template <>
+struct Random<double> {
+  using T = double;
+
+  template <typename R>
+  T static sample(R& rng) {
+    std::normal_distribution<double> rnorm;
+    return rnorm(rng);
+  }
+};
+
+template <template <typename, int = 0> class LieGroup_>
+struct LieGroupCeresTests {
+  template <typename T>
+  using LieGroup = LieGroup_<T>;
+  using LieGroupd = LieGroup<double>;
+  using Pointd = typename LieGroupd::Point;
+  using Tangentd = typename LieGroupd::Tangent;
+  template <typename T>
+  using StdVector = std::vector<T, Eigen::aligned_allocator<T>>;
+  static int constexpr N = LieGroupd::N;
+  static int constexpr num_parameters = LieGroupd::num_parameters;
+  static int constexpr DoF = LieGroupd::DoF;
+
+  struct TestLieGroupCostFunctor {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    TestLieGroupCostFunctor(const LieGroupd& T_aw) : T_aw(T_aw) {}
+
+    template <class T>
+    bool operator()(T const* const sT_wa, T* sResiduals) const {
+      Eigen::Map<LieGroup<T> const> const T_wa(sT_wa);
+      // Mapper class is only used to facciliate difference between
+      // SO2 (which uses Scalar as tangent vector type) and other groups
+      // (which use Vector<...> as tangent vector type).
+      // 
+      // Feel free to use direct dereferencing or Eigen::Map depending
+      // on ypur use-case for concrete application
+      //
+      // We only use Mapper class in order to make tests universally
+      // compatible with LieGroup::Tangent being Scalar or Vector
+      using Mapper = Mapper<typename LieGroup<T>::Tangent>;
+      typename Mapper::Map residuals = Mapper::map(sResiduals);
+
+      // We are able to mix Sophus types with doubles and Jet types withou
+      // needing to cast to T.
+      residuals = (T_aw * T_wa).log();
+      // Reverse order of multiplication. This forces the compiler to verify
+      // that (Jet, double) and (double, Jet) LieGroup multiplication work
+      // correctly.
+      residuals = (T_wa * T_aw).log();
+      // Finally, ensure that Jet-to-Jet multiplication works.
+      residuals = (T_wa * T_aw.template cast<T>()).log();
+      return true;
+    }
+
+    LieGroupd T_aw;
+  };
+  struct TestPointCostFunctor {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    TestPointCostFunctor(const LieGroupd& T_aw, const Pointd& point_a)
+        : T_aw(T_aw), point_a(point_a) {}
+
+    template <class T>
+    bool operator()(T const* const sT_wa, T const* const spoint_b,
+                    T* sResiduals) const {
+      using LieGroupT = LieGroup<T>;
+      using PointT = typename LieGroupT::Point;
+      Eigen::Map<LieGroupT const> const T_wa(sT_wa);
+      Eigen::Map<PointT const> point_b(spoint_b);
+      Eigen::Map<PointT> residuals(sResiduals);
+
+      // Multiply LieGroupd by Jet Vector3.
+      PointT point_b_prime = T_aw * point_b;
+      // Ensure Jet LieGroup multiplication with Jet Vector3.
+      point_b_prime = T_aw.template cast<T>() * point_b;
+
+      // Multiply Jet LieGroup with Vector3d.
+      PointT point_a_prime = T_wa * point_a;
+      // Ensure Jet LieGroup multiplication with Jet Vector3.
+      point_a_prime = T_wa * point_a.template cast<T>();
+
+      residuals = point_b_prime - point_a_prime;
+      return true;
+    }
+
+    LieGroupd T_aw;
+    Pointd point_a;
+  };
+
+  struct TestGraphFunctor {
+    template <typename T>
+    bool operator()(const T* a, const T* b, T* residuals) const {
+      using LieGroupT = LieGroup<T>;
+      Eigen::Map<const LieGroupT> A(a);
+      Eigen::Map<const LieGroupT> B(b);
+      using Mapper = Mapper<typename LieGroupT::Tangent>;
+      typename Mapper::Map diff_log = Mapper::map(residuals);
+
+      // Jet LieGroup multiplication with LieGroupd
+      diff_log = (diff * (B.inverse() * A)).log();
+      return true;
+    }
+
+    TestGraphFunctor(const LieGroupd& diff) : diff(diff) {}
+    const LieGroupd diff;
+  };
+
+  bool testAll() {
+    bool passed = true;
+    for (size_t i = 0; i < group_vec.size(); ++i) {
+      for (size_t j = 0; j < group_vec.size(); ++j) {
+        if (i == j) continue;
+        for (size_t k = 0; k < point_vec.size(); ++k) {
+          for (size_t l = 0; l < point_vec.size(); ++l) {
+            if (k == l) continue;
+            std::cerr << "Simple test #" << i << ", " << j << ", " << k << ", "
+                      << l;
+            passed &=
+                test(group_vec[i], group_vec[j], point_vec[k], point_vec[l]);
+            processTestResult(passed);
+          }
+        }
+      }
+    }
+    int Ns[] = {20, 40, 80, 160};
+    for (auto N : Ns) {
+      std::cerr << "Averaging test: N = " << N;
+      passed &= testAveraging(N, .5, .1);
+      processTestResult(passed);
+    }
+    return passed;
+  }
+
+  bool testAveraging(const size_t num_vertices, const double sigma_init,
+                     const double sigma_observation) {
+    if (!num_vertices) return true;
+    const double sigma_init_elementwise = sigma_init / std::sqrt(DoF);
+    const double sigma_observation_elementwise =
+        sigma_observation / std::sqrt(DoF);
+    // Running Lie group averaging on a K_n graph with a random initialization
+    // noise and random noise in observations
+    ceres::Problem problem;
+
+    // "Random" initialization in order to keep tests repeatable
+    std::mt19937 rng(2021);
+    StdVector<LieGroupd> V(num_vertices), V_estimate;
+    V_estimate.reserve(num_vertices);
+    double initial_error = 0.;
+    // All parameter blocks have the same parameterization, thus it can be
+    // reused. Memory will be freed by ceres-solver
+    auto parametrization = new Sophus::LocalParameterization<LieGroup_>;
+    // All vertices are initialized with an i.i.d noise with normal
+    // distribution; Scaling is adjusted in order to maintain the same
+    // expectation of squared norm for all groups
+    for (size_t i = 0; i < num_vertices; ++i) {
+      auto& v = V[i];
+      v = LieGroupd::sampleUniform(rng);
+      const Tangentd delta_log =
+          Random<Tangentd>::sample(rng) * sigma_init_elementwise;
+      const LieGroupd delta = LieGroupd::exp(delta_log);
+      V_estimate.emplace_back(v * delta);
+      initial_error += squaredNorm(delta_log);
+      problem.AddParameterBlock(V_estimate.back().data(),
+                                LieGroupd::num_parameters, parametrization);
+    }
+
+    // For simplicity of graph generation, we use a complete (undirected) graph.
+    // Each edge (observation) has i.i.d noise with multivariate normal
+    // distribution; Scaling is adjusted in order to maintain the same
+    // expectation of squared norm for all groups
+    for (size_t i = 0; i < num_vertices; ++i)
+      for (size_t j = i + 1; j < num_vertices; ++j) {
+        LieGroupd diff = V[i].inverse() * V[j];
+        const auto delta_log =
+            Random<typename LieGroupd::Tangent>::sample(rng) *
+            sigma_observation_elementwise;
+        const auto delta = LieGroupd::exp(delta_log);
+        ceres::CostFunction* cost =
+            new ceres::AutoDiffCostFunction<TestGraphFunctor, LieGroupd::DoF,
+                                            LieGroupd::num_parameters,
+                                            LieGroupd::num_parameters>(
+                new TestGraphFunctor(diff * delta));
+        // For real-world problems you should consider using robust
+        // loss-function
+        problem.AddResidualBlock(cost, nullptr, V_estimate[i].data(),
+                                 V_estimate[j].data());
+      }
+
+    ceres::Solver::Options options;
+    options.gradient_tolerance = 1e-2 * Sophus::Constants<double>::epsilon();
+    options.function_tolerance = 1e-2 * Sophus::Constants<double>::epsilon();
+    options.parameter_tolerance = 1e-2 * Sophus::Constants<double>::epsilon();
+    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+
+    ceres::Solver::Summary summary;
+    Solve(options, &problem, &summary);
+
+    // Computing final error in the estimates
+    double final_error = 0.;
+    for (size_t i = 0; i < num_vertices; ++i) {
+      final_error += squaredNorm((V[i].inverse() * V_estimate[i]).log());
+    }
+
+    // Expecting reasonable decrease of both estimates' errors and residuals
+    return summary.final_cost < .25 * summary.initial_cost &&
+           final_error < .25 * initial_error;
+  }
+
+  bool test(LieGroupd const& T_w_targ, LieGroupd const& T_w_init,
+            Pointd const& point_a_init, Pointd const& point_b) {
+    static constexpr int kNumPointParameters = Pointd::RowsAtCompileTime;
+
+    // Optimization parameters.
+    LieGroupd T_wr = T_w_init;
+    Pointd point_a = point_a_init;
+
+    // Build the problem.
+    ceres::Problem problem;
+
+    // Specify local update rule for our parameter
+    problem.AddParameterBlock(T_wr.data(), num_parameters,
+                              new Sophus::LocalParameterization<LieGroup_>);
+
+    // Create and add cost functions. Derivatives will be evaluated via
+    // automatic differentiation
+    ceres::CostFunction* cost_function1 =
+        new ceres::AutoDiffCostFunction<TestLieGroupCostFunctor, LieGroupd::DoF,
+                                        LieGroupd::num_parameters>(
+            new TestLieGroupCostFunctor(T_w_targ.inverse()));
+    problem.AddResidualBlock(cost_function1, nullptr, T_wr.data());
+    ceres::CostFunction* cost_function2 =
+        new ceres::AutoDiffCostFunction<TestPointCostFunctor,
+                                        kNumPointParameters, num_parameters,
+                                        kNumPointParameters>(
+            new TestPointCostFunctor(T_w_targ.inverse(), point_b));
+    problem.AddResidualBlock(cost_function2, nullptr, T_wr.data(), point_a.data());
+
+    // Set solver options (precision / method)
+    ceres::Solver::Options options;
+    options.gradient_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
+    options.function_tolerance = 0.01 * Sophus::Constants<double>::epsilon();
+    options.linear_solver_type = ceres::DENSE_QR;
+    options.max_num_iterations = 100;
+
+    // Solve
+    ceres::Solver::Summary summary;
+    Solve(options, &problem, &summary);
+
+    // Difference between target and parameter
+    double const mse = squaredNorm((T_w_targ.inverse() * T_wr).log());
+    bool const passed = mse < 10. * Sophus::Constants<double>::epsilon();
+    return passed;
+  }
+
+  LieGroupCeresTests(const StdVector<LieGroupd>& group_vec,
+                     const StdVector<Pointd>& point_vec)
+      : group_vec(group_vec), point_vec(point_vec) {}
+
+  StdVector<LieGroupd> group_vec;
+  StdVector<Pointd> point_vec;
+};
+
+}  // namespace Sophus
+
+#endif