Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1aa618d
Support Ceres 2.1+ Manifolds in backwards-compatible manner
ahojnnes Dec 10, 2024
f7bfb07
d
ahojnnes Dec 10, 2024
75bd5f8
d
ahojnnes Dec 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
60 changes: 48 additions & 12 deletions corelib/src/optimizer/OptimizerCeres.cpp
View file
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,18 +28,51 @@
//
// Author: [email protected] (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
Open in desktop
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