i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-08 02:57:46 +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
corelib/src/optimizer/gtsam/GravityFactor.h
+36 -6
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
};
corelib/src/optimizer/gtsam/XYFactor.h
+6 -1
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
corelib/src/optimizer/gtsam/XYZFactor.h
+12 -2
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();
}
};