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

Fixed build with gtsam 4.3.0 #1033

Merged
merged 2 commits into from
May 14, 2023
Merged

Fixed build with gtsam 4.3.0 #1033

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
012f7d0
Fixed build with gtsam 4.3.0
matlabbe May 14, 2023
01ef2cd
Merge branch 'master' of https://github.com/introlab/rtabmap into mat…
matlabbe May 14, 2023
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
8 changes: 8 additions & 0 deletions corelib/src/optimizer/OptimizerGTSAM.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand Down
42 changes: 36 additions & 6 deletions corelib/src/optimizer/gtsam/GravityFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,15 +63,21 @@ class GravityFactor {

/** 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
};

/**
Expand All @@ -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;
Expand All @@ -111,7 +121,11 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {

/// @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)));
}

Expand All @@ -124,7 +138,11 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {

/** 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 {
Expand All @@ -135,7 +153,7 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
}

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>
Expand All @@ -145,6 +163,7 @@ class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
ar & boost::serialization::make_nvp("GravityFactor",
boost::serialization::base_object<GravityFactor>(*this));
}
#endif

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Expand All @@ -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;

Expand All @@ -189,7 +211,11 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,

/// @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)));
}

Expand All @@ -202,7 +228,11 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,

/** 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;
Expand All @@ -219,7 +249,7 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
}

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>
Expand All @@ -229,7 +259,7 @@ class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
ar & boost::serialization::make_nvp("GravityFactor",
boost::serialization::base_object<GravityFactor>(*this));
}

#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
Expand Down
7 changes: 6 additions & 1 deletion corelib/src/optimizer/gtsam/XYFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,12 @@ class XYFactor: public gtsam::NoiseModelFactor1<VALUE> {
// 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
Expand Down
14 changes: 12 additions & 2 deletions corelib/src/optimizer/gtsam/XYZFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,14 +41,24 @@ class XYZFactor: public gtsam::NoiseModelFactor1<VALUE> {
// 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();
}
};
Expand Down
6 changes: 6 additions & 0 deletions corelib/src/optimizer/vertigo/gtsam/DerivedValue.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,9 +72,15 @@ class DerivedValue : public gtsam::Value {
/**
* 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 {
Expand Down
20 changes: 16 additions & 4 deletions corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
17 changes: 15 additions & 2 deletions corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down Expand Up @@ -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);
}
};
Expand Down
15 changes: 14 additions & 1 deletion corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down Expand Up @@ -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);
}
};
Expand Down