i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix ros gtsam 4.2a9 build (#1108)

Browse Source
This commit is contained in:
Borong Yuan
2023-08-07 22:33:23 +08:00
committed by GitHub
parent a94a4c9802
commit f0ba56bf5e
9 changed files with 26 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -527,7 +527,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{ {
float x,y,z,roll,pitch,yaw; float x,y,z,roll,pitch,yaw;
std::map<int, Transform> tmpPoses; std::map<int, Transform> tmpPoses;
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40200
for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
#else #else
for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
@@ -634,7 +634,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks()); optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
float x,y,z,roll,pitch,yaw; float x,y,z,roll,pitch,yaw;
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40200
for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
#else #else
for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)

20
corelib/src/optimizer/gtsam/GravityFactor.h
View File

@@ -63,14 +63,14 @@ public:
/** vector of errors */ /** vector of errors */
Vector attitudeError(const Rot3& p, Vector attitudeError(const Rot3& p,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
OptionalJacobian<2,3> H = {}) const; OptionalJacobian<2,3> H = {}) const;
#else #else
OptionalJacobian<2,3> H = boost::none) const; OptionalJacobian<2,3> H = boost::none) const;
#endif #endif
/** Serialization function */ /** Serialization function */
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3) #if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
@@ -91,7 +91,7 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
public: public:
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
typedef std::shared_ptr<Rot3GravityFactor> shared_ptr; typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
#else #else
typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr; typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
@@ -121,7 +121,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
return std::static_pointer_cast<gtsam::NonlinearFactor>( return std::static_pointer_cast<gtsam::NonlinearFactor>(
#else #else
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
@@ -138,7 +138,7 @@ public:
/** vector of errors */ /** vector of errors */
virtual Vector evaluateError(const Rot3& nRb, // virtual Vector evaluateError(const Rot3& nRb, //
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const { OptionalMatrixType H = OptionalNone) const {
#else #else
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {
@@ -153,7 +153,7 @@ public:
} }
private: private:
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3) #if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@@ -182,7 +182,7 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
public: public:
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
typedef std::shared_ptr<Pose3GravityFactor> shared_ptr; typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
#else #else
typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr; typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
@@ -211,7 +211,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
return std::static_pointer_cast<gtsam::NonlinearFactor>( return std::static_pointer_cast<gtsam::NonlinearFactor>(
#else #else
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
@@ -228,7 +228,7 @@ public:
/** vector of errors */ /** vector of errors */
virtual Vector evaluateError(const Pose3& nTb, // virtual Vector evaluateError(const Pose3& nTb, //
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const { OptionalMatrixType H = OptionalNone) const {
#else #else
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {
@@ -249,7 +249,7 @@ public:
} }
private: private:
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3) #if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>

2
corelib/src/optimizer/gtsam/XYFactor.h
View File

@@ -42,7 +42,7 @@ public:
// @param p the pose in Pose2 // @param p the pose in Pose2
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer // @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const VALUE& p, gtsam::Vector evaluateError(const VALUE& p,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const { OptionalMatrixType H = OptionalNone) const {
#else #else
boost::optional<gtsam::Matrix&> H = boost::none) const { boost::optional<gtsam::Matrix&> H = boost::none) const {

4
corelib/src/optimizer/gtsam/XYZFactor.h
View File

@@ -42,7 +42,7 @@ public:
// @param p the pose in Pose // @param p the pose in Pose
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer // @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose3& p, gtsam::Vector evaluateError(const gtsam::Pose3& p,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const { OptionalMatrixType H = OptionalNone) const {
#else #else
boost::optional<gtsam::Matrix&> H = boost::none) const { boost::optional<gtsam::Matrix&> H = boost::none) const {
@@ -54,7 +54,7 @@ public:
return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished(); return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
} }
gtsam::Vector evaluateError(const gtsam::Point3& p, gtsam::Vector evaluateError(const gtsam::Point3& p,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const { OptionalMatrixType H = OptionalNone) const {
#else #else
boost::optional<gtsam::Matrix&> H = boost::none) const { boost::optional<gtsam::Matrix&> H = boost::none) const {

2
corelib/src/optimizer/vertigo/gtsam/DerivedValue.h
View File

@@ -72,7 +72,7 @@ public:
/** /**
* Clone this value (normal clone on the heap, delete with 'delete' operator) * Clone this value (normal clone on the heap, delete with 'delete' operator)
*/ */
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
virtual std::shared_ptr<gtsam::Value> clone() const { virtual std::shared_ptr<gtsam::Value> clone() const {
return std::make_shared<DERIVED>(static_cast<const DERIVED&>(*this)); return std::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
} }

4
corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h
View File

@@ -12,7 +12,7 @@
#include <Eigen/Eigen> #include <Eigen/Eigen>
#include <gtsam/config.h> #include <gtsam/config.h>
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1) #if GTSAM_VERSION_NUMERIC >= 40100
namespace gtsam { namespace gtsam {
gtsam::Matrix inverse(const gtsam::Matrix & matrix) gtsam::Matrix inverse(const gtsam::Matrix & matrix)
{ {
@@ -49,7 +49,7 @@ namespace vertigo {
double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant()); double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant());
double l1 = nu1 * exp(-0.5*m1); double l1 = nu1 * exp(-0.5*m1);
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1) #if GTSAM_VERSION_NUMERIC >= 40100
double m2 = nullHypothesisModel->squaredMahalanobisDistance(error); double m2 = nullHypothesisModel->squaredMahalanobisDistance(error);
#else #else
double m2 = nullHypothesisModel->distance(error); double m2 = nullHypothesisModel->distance(error);

4
corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h
View File

@@ -30,7 +30,7 @@ namespace vertigo {
betweenFactor(key1, key2, measured, model) {}; betweenFactor(key1, key2, measured, model) {};
gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s, gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone, OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const OptionalMatrixType H3 = OptionalNone) const
@@ -70,7 +70,7 @@ namespace vertigo {
betweenFactor(key1, key2, measured, model) {}; betweenFactor(key1, key2, measured, model) {};
gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s, gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone, OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const OptionalMatrixType H3 = OptionalNone) const

6
corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h
View File

@@ -76,7 +76,7 @@ namespace vertigo {
/** between operation */ /** between operation */
inline SwitchVariableLinear between(const SwitchVariableLinear& l2, inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1=OptionalNone, OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone) const { OptionalMatrixType H2=OptionalNone) const {
#else #else
@@ -121,7 +121,7 @@ template<> struct traits<vertigo::SwitchVariableLinear> {
typedef OptionalJacobian<3, 3> ChartJacobian; typedef OptionalJacobian<3, 3> ChartJacobian;
typedef gtsam::Vector TangentVector; typedef gtsam::Vector TangentVector;
static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other, static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
#else #else
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
@@ -129,7 +129,7 @@ template<> struct traits<vertigo::SwitchVariableLinear> {
return origin.localCoordinates(other); return origin.localCoordinates(other);
} }
static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v, static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
ChartJacobian H1 = {}, ChartJacobian H2 = {}) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
#else #else
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {

6
corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h
View File

@@ -76,7 +76,7 @@ namespace vertigo {
/** between operation */ /** between operation */
inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2, inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1=OptionalNone, OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone) const { OptionalMatrixType H2=OptionalNone) const {
#else #else
@@ -122,7 +122,7 @@ template<> struct traits<vertigo::SwitchVariableSigmoid> {
typedef OptionalJacobian<3, 3> ChartJacobian; typedef OptionalJacobian<3, 3> ChartJacobian;
typedef gtsam::Vector TangentVector; typedef gtsam::Vector TangentVector;
static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other, static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
#else #else
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
@@ -130,7 +130,7 @@ template<> struct traits<vertigo::SwitchVariableSigmoid> {
return origin.localCoordinates(other); return origin.localCoordinates(other);
} }
static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v, static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3) #if GTSAM_VERSION_NUMERIC >= 40300
ChartJacobian H1 = {}, ChartJacobian H2 = {}) { ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
#else #else
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {