i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* Support Ceres 2.1+ Manifolds in backwards-compatible manner

* d

* d
This commit is contained in:
Johannes Schönberger
2024-12-14 19:27:21 +01:00
committed by GitHub
parent 38cacb7978
commit 626bf64983
3 changed files with 91 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
corelib/src/optimizer/OptimizerCeres.cpp
View File

@@ -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;
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)
{
@@ -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
{
@@ -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

48
corelib/src/optimizer/ceres/pose_graph_2d/angle_local_parameterization.h → corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h
View File

@@ -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 <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,
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
corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h
View File

@@ -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,