Ceres manifold

scripts/install_linux_deps.sh

@@ -17,7 +17,7 @@ cd ..
 
 git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver
 cd ceres-solver
-git reset --hard 399cda773035d99eaf1f4a129a666b3c4df9d1b1
+git reset --hard b0aef211db734379319c19c030e734d6e23436b0
 mkdir build
 cd build
 ccache -s

scripts/install_osx_deps.sh

@@ -2,5 +2,51 @@
 
 set -x # echo on
 set -e # exit on error
+
 brew update
-brew install ceres-solver fmt
+brew install fmt
+
+
+# Build a specific version of ceres-solver instead of one shipped over brew
+curl https://raw.githubusercontent.com/Homebrew/homebrew-core/b0792ccba6e71cd028263ca7621db894afc602d2/Formula/ceres-solver.rb -o ceres-solver.rb
+patch <<EOF
+--- ceres-solver.rb
++++ ceres-solver.rb
+@@ -1,8 +1,8 @@
+ class CeresSolver < Formula
+   desc "C++ library for large-scale optimization"
+   homepage "http://ceres-solver.org/"
+-  url "http://ceres-solver.org/ceres-solver-2.0.0.tar.gz"
+-  sha256 "10298a1d75ca884aa0507d1abb0e0f04800a92871cd400d4c361b56a777a7603"
++  url "https://github.com/ceres-solver/ceres-solver/archive/refs/tags/2.1.0rc1.tar.gz"
++  sha256 "9138a7d80a3142fe3a98519d58a489da9e204b815cd8c0571b3e643f04eca574"
+   license "BSD-3-Clause"
+   revision 4
+   head "https://ceres-solver.googlesource.com/ceres-solver.git", branch: "master"
+@@ -31,23 +31,16 @@
+   depends_on "suite-sparse"
+   depends_on "tbb"
+
+-  # Fix compatibility with TBB 2021.1
+-  # See https://github.com/ceres-solver/ceres-solver/issues/669
+-  # Remove in the next release
+-  patch do
+-    url "https://github.com/ceres-solver/ceres-solver/commit/941ea13475913ef8322584f7401633de9967ccc8.patch?full_index=1"
+-    sha256 "c61ca2ff1e92cc2134ba8e154bd9052717ba3fcae085e8f44957b9c22e6aa4ff"
+-  end
+
+   def install
+     system "cmake", ".", *std_cmake_args,
+                     "-DBUILD_SHARED_LIBS=ON",
+                     "-DBUILD_EXAMPLES=OFF",
+-                    "-DLIB_SUFFIX=''"
++                    "-DLIB_SUFFIX=''",
++                    "-DCMAKE_CXX_COMPILER_LAUNCHER=/usr/local/bin/ccache"
+     system "make"
+     system "make", "install"
+     pkgshare.install "examples", "data"
+-    doc.install "docs/html" unless build.head?
+   end
+EOF
+
+HOMEBREW_NO_INSTALLED_DEPENDENTS_CHECK=1 brew install --build-from-source ./ceres-solver.rb

sophus/cartesian.hpp

@@ -131,6 +131,15 @@ class CartesianBase {
     return m;
   }
 
+  /// Returns derivative of log(this^{-1} * x) by x at x=this.
+  ///
+  SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_log_this_inv_by_x_at_this()
+      const {
+    Matrix<Scalar, DoF, num_parameters> m;
+    m.setIdentity();
+    return m;
+  }
+
   /// Returns group inverse.
   ///
   /// The additive inverse.

sophus/ceres_local_parameterization.hpp

@@ -2,55 +2,9 @@
 
 #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&;
+#include <sophus/ceres_typetraits.hpp>
 
-  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); }
-};
+namespace Sophus {
 
 /// Templated local parameterization for LieGroup [with implemented
 /// LieGroup::Dx_this_mul_exp_x_at_0() ]

sophus/ceres_manifold.hpp

@@ -0,0 +1,68 @@
+#pragma once
+
+#include <ceres/manifold.h>
+#include <sophus/ceres_typetraits.hpp>
+
+namespace Sophus {
+
+/// Templated local parameterization for LieGroup [with implemented
+/// LieGroup::Dx_this_mul_exp_x_at_0() ]
+template <template <typename, int = 0> class LieGroup>
+class Manifold : public ceres::Manifold {
+ public:
+  using LieGroupd = LieGroup<double>;
+  using Tangent = typename LieGroupd::Tangent;
+  using TangentMap = typename Sophus::Mapper<Tangent>::Map;
+  using TangentConstMap = 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);
+    TangentConstMap 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 PlusJacobian(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;
+  }
+
+  bool Minus(double const* y_raw, double const* x_raw,
+             double* y_minus_x_raw) const override {
+    Eigen::Map<LieGroupd const> y(y_raw), x(x_raw);
+    TangentMap y_minus_x = Sophus::Mapper<Tangent>::map(y_minus_x_raw);
+
+    y_minus_x = (x.inverse() * y).log();
+    return true;
+  }
+
+  bool MinusJacobian(double const* x_raw, double* jacobian_raw) const override {
+    Eigen::Map<LieGroupd const> x(x_raw);
+    Eigen::Map<Eigen::Matrix<double, DoF, num_parameters, Eigen::RowMajor>>
+        jacobian(jacobian_raw);
+    jacobian = x.Dx_log_this_inv_by_x_at_this();
+    return true;
+  }
+
+  int AmbientSize() const override { return LieGroupd::num_parameters; }
+
+  int TangentSize() const override { return LieGroupd::DoF; }
+};
+
+}  // namespace Sophus

sophus/ceres_typetraits.hpp

@@ -0,0 +1,56 @@
+#ifndef SOPHUS_CERES_TYPETRAITS_HPP
+#define SOPHUS_CERES_TYPETRAITS_HPP
+
+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); }
+};
+
+}  // namespace Sophus
+
+#endif

sophus/rxso2.hpp

@@ -318,6 +318,16 @@ class RxSO2Base {
     return J;
   }
 
+  /// Returns derivative of log(this^{-1} * x) by x at x=this.
+  ///
+  SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
+      const {
+    Matrix<Scalar, DoF, num_parameters> J;
+    const Scalar norm_sq_inv = Scalar(1.) / complex().squaredNorm();
+    J << -complex().y(), complex().x(), complex().x(), complex().y();
+    return J * norm_sq_inv;
+  }
+
   /// Returns internal parameters of RxSO(2).
   ///
   /// It returns (c[0], c[1]), with c being the  complex number.

sophus/rxso3.hpp

@@ -459,6 +459,22 @@ class RxSO3Base {
     return J;
   }
 
+  /// Returns derivative of log(this^{-1} * x) by x at x=this.
+  ///
+  SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
+      const {
+    auto& q = quaternion();
+    Matrix<Scalar, DoF, num_parameters> J;
+    // clang-format off
+    J << q.w(),  q.z(), -q.y(), -q.x(),
+        -q.z(),  q.w(),  q.x(), -q.y(),
+         q.y(), -q.x(),  q.w(), -q.z(),
+         q.x(),  q.y(),  q.z(),  q.w();
+    // clang-format on
+    const Scalar scaler = Scalar(2.) / q.squaredNorm();
+    return J * scaler;
+  }
+
  private:
   /// Mutator of quaternion is private to ensure class invariant.
   ///

sophus/se2.hpp

@@ -140,6 +140,18 @@ class SE2Base {
     return J;
   }
 
+  /// Returns derivative of log(this^{-1} * x) by x at x=this.
+  ///
+  SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
+      const {
+    Matrix<Scalar, DoF, num_parameters> J;
+    J.template block<2, 2>(0, 0).setZero();
+    J.template block<2, 2>(0, 2) = so2().inverse().matrix();
+    J.template block<1, 2>(2, 0) = so2().Dx_log_this_inv_by_x_at_this();
+    J.template block<1, 2>(2, 2).setZero();
+    return J;
+  }
+
   /// Returns group inverse.
   ///
   SOPHUS_FUNC SE2<Scalar> inverse() const {
@@ -510,8 +522,10 @@ class SE2 : public SE2Base<SE2<Scalar_, Options>> {
       Scalar const i(1);
 
       // clang-format off
-      J << o, o, o, o, o, i, i, o, -Scalar(0.5) * upsilon[1], o, i,
-          Scalar(0.5) * upsilon[0];
+      J << o, o, o,
+           o, o, i,
+           i, o, -Scalar(0.5) * upsilon[1],
+           o, i,  Scalar(0.5) * upsilon[0];
       // clang-format on
       return J;
     }
@@ -554,7 +568,10 @@ class SE2 : public SE2Base<SE2<Scalar_, Options>> {
     Scalar const i(1);
 
     // clang-format off
-    J << o, o, o, o, o, i, i, o, o, o, i, o;
+    J << o, o, o,
+         o, o, i,
+         i, o, o,
+         o, i, o;
     // clang-format on
     return J;
   }

sophus/se3.hpp

@@ -203,6 +203,18 @@ class SE3Base {
     return J;
   }
 
+  /// Returns derivative of log(this^{-1} * x) by x at x=this.
+  ///
+  SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
+      const {
+    Matrix<Scalar, DoF, num_parameters> J;
+    J.template block<3, 4>(0, 0).setZero();
+    J.template block<3, 3>(0, 4) = so3().inverse().matrix();
+    J.template block<3, 4>(3, 0) = so3().Dx_log_this_inv_by_x_at_this();
+    J.template block<3, 3>(3, 4).setZero();
+    return J;
+  }
+
   /// Returns group inverse.
   ///
   SOPHUS_FUNC SE3<Scalar> inverse() const {
@@ -794,12 +806,13 @@ class SE3 : public SE3Base<SE3<Scalar_, Options>> {
     Scalar const i(1);
 
     // clang-format off
-    J << o, o, o, h, o, o, o,
-         o, o, o, h, o, o, o,
-         o, o, o, h, o, o, o,
-         o, o, o, i, o, o, o,
-         o, o, o, i, o, o, o,
-         o, o, o, i, o, o, o;
+    J << o, o, o, h, o, o,
+         o, o, o, o, h, o,
+	 o, o, o, o, o, h,
+	 o, o, o, o, o, o,
+	 i, o, o, o, o, o,
+	 o, i, o, o, o, o,
+	 o, o, i, o, o, o;
     // clang-format on
     return J;
   }