From 1aa618d79804fcf11342923b66774ff53a1490d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Tue, 10 Dec 2024 08:35:25 +0100 Subject: [PATCH] Support Ceres 2.1+ Manifolds in backwards-compatible manner --- corelib/src/optimizer/OptimizerCeres.cpp | 56 +++++++++++++++---- ...al_parameterization.h => angle_manifold.h} | 48 +++++++++++++--- .../eigen_quaternion_parameterization.h | 4 +- 3 files changed, 87 insertions(+), 21 deletions(-) rename corelib/src/optimizer/ceres/pose_graph_2d/{angle_local_parameterization.h => angle_manifold.h} (66%) diff --git a/corelib/src/optimizer/OptimizerCeres.cpp b/corelib/src/optimizer/OptimizerCeres.cpp index 5a62283a83..23f0842381 100644 --- a/corelib/src/optimizer/OptimizerCeres.cpp +++ b/corelib/src/optimizer/OptimizerCeres.cpp @@ -37,22 +37,43 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef RTABMAP_CERES #include +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) +#include +#else #include +#endif #include "ceres/pose_graph_2d/types.h" #include "ceres/pose_graph_2d/pose_graph_2d_error_term.h" -#include "ceres/pose_graph_2d/angle_local_parameterization.h" +#include "ceres/pose_graph_2d/angle_manifold.h" #include "ceres/pose_graph_3d/types.h" #include "ceres/pose_graph_3d/pose_graph_3d_error_term.h" #include "ceres/bundle/BAProblem.h" #include "ceres/bundle/snavely_reprojection_error.h" #if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12)) -#include "ceres/pose_graph_3d/eigen_quaternion_parameterization.h" +#include "ceres/pose_graph_3d/eigen_quaternion_manifold.h" #endif #endif namespace rtabmap { +namespace { + +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) +inline void SetCeresProblemManifold(ceres::Problem* problem, double* params, + ceres::Manifold* manifold) { + problem->SetManifold(params, manifold); +#else +inline void SetCeresProblemManifold( + ceres::Problem* problem, double* params, + ceres::LocalParameterization* parameterization) { + problem->SetParameterization(params, parameterization); +#endif +} + +} // namespace bool OptimizerCeres::available() { @@ -118,8 +139,14 @@ std::map OptimizerCeres::optimize( } ceres::LossFunction* loss_function = NULL; - ceres::LocalParameterization* angle_local_parameterization = NULL; - ceres::LocalParameterization* quaternion_local_parameterization = NULL; +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) + ceres::Manifold* angle_local_manifold = NULL; + ceres::Manifold* quaternion_local_manifold = NULL; +#else + ceres::LocalParameterization* angle_local_manifold = NULL; + ceres::LocalParameterization* quaternion_local_manifold = NULL; +#endif for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) { @@ -164,12 +191,12 @@ std::map OptimizerCeres::optimize( &pose_begin_iter->second.x, &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians, &pose_end_iter->second.x, &pose_end_iter->second.y, &pose_end_iter->second.yaw_radians); - if(angle_local_parameterization == NULL) + if(angle_local_manifold == NULL) { - angle_local_parameterization = ceres::examples::AngleLocalParameterization::Create(); + angle_local_manifold = ceres::examples::AngleLocalParameterization::Create(); } - problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization); - problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization); + SetCeresProblemManifold(problem, &pose_begin_iter->second.yaw_radians, angle_local_manifold); + SetCeresProblemManifold(problem, &pose_end_iter->second.yaw_radians, angle_local_manifold); } else { @@ -194,12 +221,17 @@ std::map OptimizerCeres::optimize( problem.AddResidualBlock(cost_function, loss_function, pose_begin_iter->second.p.data(), pose_begin_iter->second.q.coeffs().data(), pose_end_iter->second.p.data(), pose_end_iter->second.q.coeffs().data()); - if(quaternion_local_parameterization == NULL) + if(quaternion_local_manifold == NULL) { - quaternion_local_parameterization = new ceres::EigenQuaternionParameterization; +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) + quaternion_local_manifold = new ceres::EigenQuaternionManifold; +#else + quaternion_local_manifold = new ceres::EigenQuaternionParameterization; +#endif } - problem.SetParameterization(pose_begin_iter->second.q.coeffs().data(), quaternion_local_parameterization); - problem.SetParameterization(pose_end_iter->second.q.coeffs().data(), quaternion_local_parameterization); + SetCeresProblemManifold(problem, pose_begin_iter->second.q.coeffs().data(), quaternion_local_manifold); + SetCeresProblemManifold(problem, pose_end_iter->second.q.coeffs().data(), quaternion_local_manifold); } } //else // not supporting pose prior and landmarks diff --git a/corelib/src/optimizer/ceres/pose_graph_2d/angle_local_parameterization.h b/corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h similarity index 66% rename from corelib/src/optimizer/ceres/pose_graph_2d/angle_local_parameterization.h rename to corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h index 428ccccdcd..b96907be7e 100644 --- a/corelib/src/optimizer/ceres/pose_graph_2d/angle_local_parameterization.h +++ b/corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h @@ -28,10 +28,11 @@ // // Author: vitus@google.com (Michael Vitus) -#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ -#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ +#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_ +#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_ -#include "ceres/local_parameterization.h" +#include "ceres/autodiff_manifold.h" +#include "ceres/manifold.h" #include "normalize_angle.h" namespace ceres { @@ -39,7 +40,39 @@ namespace examples { // Defines a local parameterization for updating the angle to be constrained in // [-pi to pi). -class AngleLocalParameterization { + +#if CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1) + +// Defines a manifold for updating the angle to be constrained in [-pi to pi). +class AngleManifold { + public: + template + bool Plus(const T* x_radians, + const T* delta_radians, + T* x_plus_delta_radians) const { + *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians); + return true; + } + + template + bool Minus(const T* y_radians, + const T* x_radians, + T* y_minus_x_radians) const { + *y_minus_x_radians = + NormalizeAngle(*y_radians) - NormalizeAngle(*x_radians); + + return true; + } + + static ceres::Manifold* Create() { + return new ceres::AutoDiffManifold; + } +}; + +#else + +class AngleManfold { public: template @@ -52,12 +85,13 @@ class AngleLocalParameterization { } static ceres::LocalParameterization* Create() { - return (new ceres::AutoDiffLocalParameterization); + return (new ceres::AutoDiffLocalParameterization); } }; +#endif + } // namespace examples } // namespace ceres -#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_ +#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_ diff --git a/corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h b/corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h index ae03ebda5c..ffab304238 100644 --- a/corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h +++ b/corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h @@ -31,7 +31,7 @@ #ifndef CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_ #define CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_ -#include "ceres/local_parameterization.h" +#include "ceres/manifold.h" namespace ceres { @@ -46,7 +46,7 @@ namespace ceres { // // Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x // with * being the quaternion multiplication operator. -class EigenQuaternionParameterization : public ceres::LocalParameterization { +class EigenQuaternionParameterization : public ceres::Manifold { public: virtual ~EigenQuaternionParameterization() {} virtual bool Plus(const double* x_ptr,