Merge pull request artivis#295 from mattalvarado/feature/update_to_ce…

…res_manifold Support Ceres 2.2+
traversaro · Jun 29, 2024 · 3b100aa · 3b100aa
2 parents 03c497d + 17b7a2e
commit 3b100aa
Show file tree

Hide file tree

Showing 12 changed files with 329 additions and 119 deletions.
diff --git a/.github/workflows/ci.yml b/.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: [
           {
@@ -60,10 +60,7 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        # Xcode 10.3 & Xcode 12.2
-        # removing macos-11.0 for now, see
-        #https://github.com/actions/virtual-environments/issues/841
-        os: [macos-11, macos-12]
+        os: [macos-12, macos-13, macos-14]
     steps:
       - name: Checkout
         uses: actions/checkout@v2
@@ -193,7 +190,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
@@ -256,7 +259,7 @@ jobs:
       fail-fast: false
       matrix:
         platform: [macos-latest, ubuntu-latest] #windows-latest,
-        python-version: ['3.7', '3.8', '3.9', '3.10']
+        python-version: ['3.8', '3.9', '3.10', '3.12']
     runs-on: ${{ matrix.platform }}
     steps:
       - name: Checkout
@@ -287,7 +290,7 @@ jobs:
     needs: [build-ubuntu, build-mac]
     strategy:
       matrix:
-        platform: [ubuntu-20.04, ubuntu-latest]
+        platform: [ubuntu-20.04, ubuntu-22.04, ubuntu-latest]
     runs-on: ${{ matrix.platform }}
     steps:
       - name: Checkout
@@ -316,21 +319,3 @@ jobs:
         run: sudo make install
       - name: Test Import
         run: python3 -c 'import manifpy'
-
-  # arm64:
-  #   needs: [build-ubuntu]
-  #   runs-on: ubuntu-latest
-  #   steps:
-  #     - name: Checkout
-  #       uses: actions/checkout@v2
-  #     - name: Setup
-  #       run: mkdir ${{runner.workspace}}/build
-  #     - name: Configure CMake
-  #       working-directory: ${{runner.workspace}}/build
-  #       run: cmake $GITHUB_WORKSPACE -DENABLE_COVERAGE=ON -DBUILD_TESTING=ON
-  #     - name: Build
-  #       working-directory: ${{runner.workspace}}/build
-  #       run: make
-  #     - name: Test
-  #       working-directory: ${{runner.workspace}}/build
-  #       run: make test
diff --git a/docs/pages/cpp/On-the-use-with-Ceres.md b/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;

diff --git a/include/manif/ceres/ceres.h b/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
 

diff --git a/include/manif/ceres/ceres_utils.h b/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.

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

diff --git a/include/manif/ceres/manifold.h b/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_ */
diff --git a/include/manif/impl/se3/SE3.h b/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)

diff --git a/test/CMakeLists.txt b/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)