Update ceres::LocalParameterization to ceres::Manifold

.github/workflows/ci.yml

@@ -19,7 +19,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        os: [ubuntu-20.04, ubuntu-22.04]
+        os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04]
         build_type: [Release, Debug]
         compiler: [
           {
@@ -193,7 +193,13 @@ jobs:
 
   ceres:
     needs: [build-ubuntu]
-    runs-on: ubuntu-latest
+    # runs-on: ubuntu-latest
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      # Test for both Ceres pre/post 2.2
+      matrix:
+        os: [ubuntu-22.04, ubuntu-24.04]
     steps:
       - name: Checkout
         uses: actions/checkout@v2

docs/pages/cpp/On-the-use-with-Ceres.md

@@ -94,8 +94,8 @@ ceres::Problem problem(problem_options);
 problem->AddParameterBlock(my_state.data(), 4);
 
 // Associate a LocalParameterization to the state vector
-problem_->SetParameterization(my_state.data(),
-                              new EigenQuaternionParameterization() );
+problem_->SetManifold(my_state.data(),
+                      new EigenQuaternionParameterization() );
 ```
 
 The `LocalParameterization` class (and derived) performs the state update step
@@ -179,7 +179,7 @@ In this example, we compute an average point from 4 points in `SE2`.
 // Tell ceres not to take ownership of the raw pointers
 ceres::Problem::Options problem_options;
 problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
-problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+problem_options.manifold_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
 
 ceres::Problem problem(problem_options);
 
@@ -217,12 +217,12 @@ problem.AddResidualBlock( obj_3_pi_over_4.get(),
 // We use a second manif helper that creates a ceres local parameterization
 // for our optimized state block.
 
-std::shared_ptr<ceres::LocalParameterization>
-  auto_diff_local_parameterization =
-    manif::make_local_parametrization_autodiff<SE2d>();
+std::shared_ptr<ceres::Manifold>
+  auto_diff_manifold =
+    manif::make_manifold_autodiff<SE2d>();
 
-problem.SetParameterization( average_state.data(),
-                             auto_diff_local_parameterization.get() );
+problem.SetManifold( average_state.data(),
+                     auto_diff_manifold.get() );
 
 // Run the solver!
 ceres::Solver::Options options;

include/manif/ceres/ceres.h

@@ -3,7 +3,11 @@
 
 #include "manif/ceres/constants.h"
 #include "manif/ceres/constraint.h"
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
+#include "manif/ceres/manifold.h"
+#else
 #include "manif/ceres/local_parametrization.h"
+#endif
 #include "manif/ceres/objective.h"
 #include "manif/ceres/ceres_utils.h"
 
@@ -18,41 +22,57 @@ struct is_ad<ceres::Jet<_Scalar, _N>> : std::integral_constant<bool, true> { };
 
 #ifdef _MANIF_MANIF_SO2_H_
 using CeresConstraintSO2 = CeresConstraintFunctor<SO2d>;
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
+using CeresManifoldSO2 = CeresManifoldFunctor<SO2d>;
+#else
 using CeresLocalParameterizationSO2 = CeresLocalParameterizationFunctor<SO2d>;
+#endif
 using CeresObjectiveSO2 = CeresObjectiveFunctor<SO2d>;
 #else
 using CeresConstraintSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
-using CeresLocalParameterizationSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
+using CeresManifoldSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
 using CeresObjectiveSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
 #endif
 
 #ifdef _MANIF_MANIF_SO3_H_
 using CeresConstraintSO3 = CeresConstraintFunctor<SO3d>;
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
+using CeresManifoldSO3 = CeresManifoldFunctor<SO3d>;
+#else
 using CeresLocalParameterizationSO3 = CeresLocalParameterizationFunctor<SO3d>;
+#endif
 using CeresObjectiveSO3 = CeresObjectiveFunctor<SO3d>;
 #else
 using CeresConstraintSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
-using CeresLocalParameterizationSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
+using CeresManifoldSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
 using CeresObjectiveSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
 #endif
 
 #ifdef _MANIF_MANIF_SE2_H_
 using CeresConstraintSE2 = CeresConstraintFunctor<SE2d>;
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
+using CeresManifoldSE2 = CeresManifoldFunctor<SE2d>;
+#else
 using CeresLocalParameterizationSE2 = CeresLocalParameterizationFunctor<SE2d>;
+#endif
 using CeresObjectiveSE2 = CeresObjectiveFunctor<SE2d>;
 #else
 using CeresConstraintSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
-using CeresLocalParameterizationSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
+using CeresManifoldSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
 using CeresObjectiveSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
 #endif
 
 #ifdef _MANIF_MANIF_SE3_H_
 using CeresConstraintSE3 = CeresConstraintFunctor<SE3d>;
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
+using CeresManifoldSE3 = CeresManifoldFunctor<SE3d>;
+#else
 using CeresLocalParameterizationSE3 = CeresLocalParameterizationFunctor<SE3d>;
+#endif
 using CeresObjectiveSE3 = CeresObjectiveFunctor<SE3d>;
 #else
 using CeresConstraintSE3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
-using CeresLocalParameterizationSE3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
+using CeresManifoldSE3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
 using CeresObjectiveSE3= internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
 #endif
 

include/manif/ceres/ceres_utils.h

@@ -1,15 +1,39 @@
 #ifndef _MANIF_MANIF_CERES_UTILS_H_
 #define _MANIF_MANIF_CERES_UTILS_H_
 
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
+#include "manif/ceres/manifold.h"
+#else
 #include "manif/ceres/local_parametrization.h"
+#endif
 #include "manif/ceres/objective.h"
 #include "manif/ceres/constraint.h"
 
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
+#include <ceres/autodiff_manifold.h>
+#else
 #include <ceres/autodiff_local_parameterization.h>
+#endif
 #include <ceres/autodiff_cost_function.h>
 
 namespace manif {
 
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
+/**
+ * @brief Helper function to create a Ceres Manifold parameterization wrapper.
+ * @see CeresManifoldFunctor
+ */
+template <typename _LieGroup>
+std::shared_ptr<
+  ceres::AutoDiffManifold<CeresManifoldFunctor<_LieGroup>,
+  _LieGroup::RepSize, _LieGroup::DoF>>
+make_manifold_autodiff()
+{
+  return std::make_shared<
+      ceres::AutoDiffManifold<
+        CeresManifoldFunctor<_LieGroup>, _LieGroup::RepSize, _LieGroup::DoF>>();
+}
+#else
 /**
  * @brief Helper function to create a Ceres autodiff local parameterization wrapper.
  * @see CeresLocalParameterizationFunctor
@@ -24,6 +48,7 @@ make_local_parameterization_autodiff()
       ceres::AutoDiffLocalParameterization<
         CeresLocalParameterizationFunctor<_LieGroup>, _LieGroup::RepSize, _LieGroup::DoF>>();
 }
+#endif
 
 /**
  * @brief Helper function to create a Ceres autodiff objective wrapper.

include/manif/ceres/constants.h

@@ -4,6 +4,7 @@
 #include "manif/constants.h"
 
 #include <ceres/jet.h>
+#include <ceres/version.h>
 
 namespace manif {
 

include/manif/ceres/manifold.h

@@ -0,0 +1,61 @@
+#ifndef _MANIF_MANIF_CERES_MANIFOLD_H_
+#define _MANIF_MANIF_CERES_MANIFOLD_H_
+
+#include <Eigen/Core>
+
+namespace manif {
+
+/**
+ * @brief A wrapper for Ceres autodiff local parameterization.
+ */
+template <typename _LieGroup>
+class CeresManifoldFunctor
+{
+  using LieGroup = _LieGroup;
+  using Tangent  = typename _LieGroup::Tangent;
+
+  template <typename _Scalar>
+  using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>;
+
+  template <typename _Scalar>
+  using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>;
+
+public:
+
+  CeresManifoldFunctor() = default;
+  virtual ~CeresManifoldFunctor() = default;
+
+  template <typename T>
+  bool Plus(const T* state_raw,
+            const T* delta_raw,
+            T* state_plus_delta_raw) const
+  {
+    const Eigen::Map<const LieGroupTemplate<T>> state(state_raw);
+    const Eigen::Map<const TangentTemplate<T>>  delta(delta_raw);
+
+    Eigen::Map<LieGroupTemplate<T>> state_plus_delta(state_plus_delta_raw);
+
+    state_plus_delta = state + delta;
+
+    return true;
+  }
+
+  template <typename T>
+  bool Minus(const T* y_raw,
+             const T* x_raw,
+             T* y_minus_x_raw) const
+  {
+    const Eigen::Map<const LieGroupTemplate<T>> y(y_raw);
+    const Eigen::Map<const LieGroupTemplate<T>>  x(x_raw);
+
+    Eigen::Map<TangentTemplate<T>> y_minus_x(y_minus_x_raw);
+
+    y_minus_x = y - x;
+
+    return true;
+  }
+};
+
+} /* namespace manif */
+
+#endif /* _MANIF_MANIF_CERES_MANIFOLD_H_ */

include/manif/impl/se3/SE3.h

@@ -154,12 +154,17 @@ SE3<_Scalar>::SE3(const LieGroupBase<_DerivedOther>& o)
   //
 }
 
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Warray-bounds"
+// Temporarily disable this warning which causes false positive with GCC 12
+// See e.g. https://gcc.gnu.org/bugzilla/show_bug.cgi?id=106247
 template <typename _Scalar>
 SE3<_Scalar>::SE3(const Translation& t, const Eigen::Quaternion<Scalar>& q)
   : SE3((DataType() << t, q.coeffs() ).finished())
 {
   //
 }
+#pragma GCC diagnostic pop
 
 template <typename _Scalar>
 SE3<_Scalar>::SE3(const Translation& t, const Eigen::AngleAxis<Scalar>& a)

test/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.8)
+cmake_minimum_required(VERSION 3.5.1)
 
 find_package(GTest QUIET)