Support Ceres 2.1+ Manifolds in backwards-compatible manner (#1405)

* Support Ceres 2.1+ Manifolds in backwards-compatible manner

* d

* d

corelib/src/optimizer/OptimizerCeres.cpp

@@ -37,22 +37,47 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #ifdef RTABMAP_CERES
 #include <ceres/ceres.h>
+
+#if CERES_VERSION_MAJOR >= 3 || \
+    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
+#include <ceres/manifold.h>
+#else
 #include <ceres/local_parameterization.h>
+#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 {
+
+#ifdef RTABMAP_CERES
+#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
+}
+#endif
+
+}  // namespace
 
 bool OptimizerCeres::available()
 {
@@ -118,8 +143,14 @@ std::map<int, Transform> OptimizerCeres::optimize(
 		}
 
 		ceres::LossFunction* loss_function = NULL;
-		ceres::LocalParameterization* angle_local_parameterization = NULL;
+#if CERES_VERSION_MAJOR >= 3 || \
-		ceres::LocalParameterization* quaternion_local_parameterization = NULL;
+    (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<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
@@ -164,12 +195,12 @@ std::map<int, Transform> 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::AngleManfold::Create();
 					}
-					problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization);
+					SetCeresProblemManifold(problem, &pose_begin_iter->second.yaw_radians, angle_local_manifold);
-					problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization);
+					SetCeresProblemManifold(problem, &pose_end_iter->second.yaw_radians, angle_local_manifold);
 				}
 				else
 				{
@@ -194,12 +225,17 @@ std::map<int, Transform> 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);
+					SetCeresProblemManifold(problem, pose_begin_iter->second.q.coeffs().data(), quaternion_local_manifold);
-					problem.SetParameterization(pose_end_iter->second.q.coeffs().data(), quaternion_local_parameterization);
+					SetCeresProblemManifold(problem, pose_end_iter->second.q.coeffs().data(), quaternion_local_manifold);
 				}
 			}
 			//else // not supporting pose prior and landmarks

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_
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_
-#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_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 <typename T>
+  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 <typename T>
+  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<AngleManifold, 1, 1>;
+  }
+};
+
+#else
+
+class AngleManfold {
  public:
 
   template <typename T>
@@ -52,12 +85,13 @@ class AngleLocalParameterization {
   }
 
   static ceres::LocalParameterization* Create() {
-    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
+    return (new ceres::AutoDiffLocalParameterization<AngleManfold, 1, 1>);
-                                                     1, 1>);
   }
 };
 
+#endif
+
 }  // namespace examples
 }  // namespace ceres
 
-#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+#endif  // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_MANIFOLD_H_

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,