Skip to content

Commit

Permalink
Support Ceres 2.1+ Manifolds in backwards-compatible manner (#1405)
Browse files Browse the repository at this point in the history 
* Support Ceres 2.1+ Manifolds in backwards-compatible manner

* d

* d
  • Loading branch information
@ahojnnes
ahojnnes authored Dec 14, 2024
1 parent 38cacb7 commit 626bf64
Show file tree
Hide file tree
Showing 3 changed files with 91 additions and 21 deletions.
60 changes: 48 additions & 12 deletions corelib/src/optimizer/OptimizerCeres.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand Down Expand Up @@ -118,8 +143,14 @@ std::map<int, Transform> 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<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
Expand Down Expand Up @@ -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);
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
{
Expand All @@ -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);
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
Expand Down
48 changes: 41 additions & 7 deletions ...e_graph_2d/angle_local_parameterization.h → ...izer/ceres/pose_graph_2d/angle_manifold.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,18 +28,51 @@
//
// 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 {
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>
Expand All @@ -52,12 +85,13 @@ class AngleLocalParameterization {
}

static ceres::LocalParameterization* Create() {
return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
1, 1>);
return (new ceres::AutoDiffLocalParameterization<AngleManfold, 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_
4 changes: 2 additions & 2 deletions corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {

Expand All @@ -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,
Expand Down

0 comments on commit 626bf64

Please sign in to comment.