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

Fixed build with gtsam 4.3.0 (#1033)

Browse Source
This commit is contained in:
matlabbe
2023-05-14 13:14:53 -07:00
committed by GitHub
parent ba33c080bc
commit 682d54725a
8 changed files with 113 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@@ -63,15 +63,21 @@ public:
/** vector of errors */
Vector attitudeError(const Rot3& p,
OptionalJacobian<2,3> H = boost::none) const;
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
OptionalJacobian<2,3> H = {}) const;
#else
OptionalJacobian<2,3> H = boost::none) const;
#endif
/** Serialization function */
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3)
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
}
#endif
};
/**
@@ -85,7 +91,11 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
public:
/// shorthand for a smart pointer to a factor
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
#else
typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
#endif
/// Typedef to this class
typedef Rot3GravityFactor This;
@@ -111,7 +121,11 @@ public:
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
return std::static_pointer_cast<gtsam::NonlinearFactor>(
#else
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
#endif
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
@@ -124,7 +138,11 @@ public:
/** vector of errors */
virtual Vector evaluateError(const Rot3& nRb, //
boost::optional<Matrix&> H = boost::none) const {
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<Matrix&> H = boost::none) const {
#endif
return attitudeError(nRb, H);
}
Unit3 nZ() const {
@@ -135,7 +153,7 @@ public:
}
private:
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3)
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
@@ -145,6 +163,7 @@ private:
ar & boost::serialization::make_nvp("GravityFactor",
boost::serialization::base_object<GravityFactor>(*this));
}
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -163,8 +182,11 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
public:
/// shorthand for a smart pointer to a factor
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
#else
typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
#endif
/// Typedef to this class
typedef Pose3GravityFactor This;
@@ -189,7 +211,11 @@ public:
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
return std::static_pointer_cast<gtsam::NonlinearFactor>(
#else
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
#endif
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
@@ -202,7 +228,11 @@ public:
/** vector of errors */
virtual Vector evaluateError(const Pose3& nTb, //
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<Matrix&> H = boost::none) const {
#endif
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;
@@ -219,7 +249,7 @@ public:
}
private:
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_MAJOR < 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR < 3)
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
@@ -229,7 +259,7 @@ private:
ar & boost::serialization::make_nvp("GravityFactor",
boost::serialization::base_object<GravityFactor>(*this));
}
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

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

@@ -41,7 +41,12 @@ public:
// error function
// @param p the pose in Pose2
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const VALUE& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
gtsam::Vector evaluateError(const VALUE& p,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H = boost::none) const {
#endif
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists

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

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

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

@@ -72,9 +72,15 @@ public:
/**
* 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)
virtual std::shared_ptr<gtsam::Value> clone() const {
return std::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
}
#else
virtual boost::shared_ptr<gtsam::Value> clone() const {
return boost::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
}
#endif
/// equals implementing generic Value interface
virtual bool equals_(const gtsam::Value& p, double tol = 1e-9) const {

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

@@ -30,9 +30,15 @@ namespace vertigo {
betweenFactor(key1, key2, measured, model) {};
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)
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const
#else
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none) const
boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none) const
#endif
{
// calculate error
@@ -64,9 +70,15 @@ namespace vertigo {
betweenFactor(key1, key2, measured, model) {};
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)
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const
#else
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none) const
boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none) const
#endif
{
// calculate error

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

@@ -76,8 +76,13 @@ namespace vertigo {
/** between operation */
inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
#endif
if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
return SwitchVariableLinear(l2.value() - value());
@@ -116,11 +121,19 @@ template<> struct traits<vertigo::SwitchVariableLinear> {
typedef OptionalJacobian<3, 3> ChartJacobian;
typedef gtsam::Vector TangentVector;
static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
#else
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
#endif
return origin.localCoordinates(other);
}
static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
#else
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
#endif
return g.retract(v);
}
};

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

@@ -76,8 +76,13 @@ namespace vertigo {
/** between operation */
inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
#endif
if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
return SwitchVariableSigmoid(l2.value() - value());
@@ -117,11 +122,19 @@ template<> struct traits<vertigo::SwitchVariableSigmoid> {
typedef OptionalJacobian<3, 3> ChartJacobian;
typedef gtsam::Vector TangentVector;
static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
#else
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
#endif
return origin.localCoordinates(other);
}
static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR == 4 && GTSAM_VERSION_MINOR >= 3)
ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
#else
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
#endif
return g.retract(v);
}
};