mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-07 10:37:47 +08:00
Correct the Jacobian calculation of GravityFactor (#1692)
* correct the Jacobian calculation of GravityFactor * reimplement GravityFactor's analytical Jacobian * adopt some minor changes from the GTSAM mainline * use GTSAM's native AttitudeFactor instead of GravityFactor * add missing header file and namespace prefix * Removed GravityFactor. Fixed build with latest GTSAM version from source. Require c++17 when building with GTSAM>=4.3.0. * Fixed cmake error when gtsam is not installed --------- Co-authored-by: matlabbe <[email protected]>
This commit is contained in:
Borong Yuan
@@ -1,91 +0,0 @@
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* Author: Mathieu Labbe
|
||||
* This file is a copy of AttitudeFactor.cpp of gtsam library but
|
||||
* with attitudeError() function overridden to ignore yaw errors.
|
||||
* For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file GravityFactor.cpp
|
||||
* @author Frank Dellaert
|
||||
* @brief Implementation file for Attitude factor
|
||||
* @date January 28, 2014
|
||||
**/
|
||||
|
||||
#include "GravityFactor.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
//***************************************************************************
|
||||
Vector GravityFactor::attitudeError(const Rot3& nRb,
|
||||
OptionalJacobian<2, 3> H) const {
|
||||
if (H) {
|
||||
Matrix23 D_nRef_R;
|
||||
Matrix22 D_e_nRef;
|
||||
Vector3 r = nRb.xyz();
|
||||
Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0).rotate(bRef_, D_nRef_R);
|
||||
Vector e = nZ_.error(nRef, D_e_nRef);
|
||||
(*H) = D_e_nRef * D_nRef_R;
|
||||
//printf("ref=%f %f %f grav=%f %f %f e= %f %f H=%f %f %f, %f %f %f\n",
|
||||
// nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1),
|
||||
// (*H)(0,0), (*H)(0,1), (*H)(0,2), (*H)(1,0), (*H)(1,1), (*H)(1,2));
|
||||
return e;
|
||||
} else {
|
||||
Vector3 r = nRb.xyz();
|
||||
Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0) * bRef_;
|
||||
Vector e = nZ_.error(nRef);
|
||||
//printf("ref=%f %f %f grav=%f %f %f e= %f %f\n", nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1));
|
||||
return e;
|
||||
}
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void Rot3GravityFactor::print(const string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "Rot3GravityFactor on " << keyFormatter(this->key()) << "\n";
|
||||
nZ_.print(" measured direction in nav frame: ");
|
||||
bRef_.print(" reference direction in body frame: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool Rot3GravityFactor::equals(const NonlinearFactor& expected,
|
||||
double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
|
||||
&& this->bRef_.equals(e->bRef_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void Pose3GravityFactor::print(const string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "Pose3GravityFactor on " << keyFormatter(this->key()) << "\n";
|
||||
nZ_.print(" measured direction in nav frame: ");
|
||||
bRef_.print(" reference direction in body frame: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool Pose3GravityFactor::equals(const NonlinearFactor& expected,
|
||||
double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
|
||||
&& this->bRef_.equals(e->bRef_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
|
||||
}/// namespace gtsam
|
||||
@@ -1,271 +0,0 @@
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* Author: Mathieu Labbe
|
||||
* This file is a copy of AttitudeFactor.h of gtsam library but
|
||||
* with attitudeError() function overridden to ignore yaw errors.
|
||||
* For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file Pose3GravityFactor.h
|
||||
* @author Frank Dellaert
|
||||
* @brief Header file for Attitude factor
|
||||
* @date January 28, 2014
|
||||
**/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300 && defined(GTSAM_WITH_NOISE_MODEL_FACTOR_N)
|
||||
#include <gtsam/nonlinear/NoiseModelFactorN.h>
|
||||
#endif
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
/**
|
||||
* Base class for prior on gravity
|
||||
* Example:
|
||||
* - measurement is direction of gravity in navigation frame nG
|
||||
* - reference is direction of z axis in body frame bF
|
||||
* This factor will give zero error if nG is opposite direction of bF
|
||||
* @addtogroup Navigation
|
||||
*/
|
||||
class GravityFactor {
|
||||
|
||||
protected:
|
||||
|
||||
const Unit3 nZ_, bRef_; ///< Position measurement in
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
GravityFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* @param nZ measured direction in navigation frame
|
||||
* @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1])
|
||||
*/
|
||||
GravityFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
nZ_(nZ), bRef_(bRef) {
|
||||
}
|
||||
|
||||
/** vector of errors */
|
||||
Vector attitudeError(const Rot3& p,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalJacobian<2,3> H = {}) const;
|
||||
#else
|
||||
OptionalJacobian<2,3> H = boost::none) const;
|
||||
#endif
|
||||
|
||||
/** Serialization function */
|
||||
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
|
||||
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
|
||||
};
|
||||
|
||||
/**
|
||||
* Version of GravityFactor for Rot3
|
||||
* @addtogroup Navigation
|
||||
*/
|
||||
class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
|
||||
|
||||
typedef NoiseModelFactor1<Rot3> Base;
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
|
||||
#else
|
||||
typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
|
||||
#endif
|
||||
|
||||
/// Typedef to this class
|
||||
typedef Rot3GravityFactor This;
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
Rot3GravityFactor() {
|
||||
}
|
||||
|
||||
virtual ~Rot3GravityFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* @param key of the Rot3 variable that will be constrained
|
||||
* @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
|
||||
* @param model Gaussian noise model
|
||||
* @param bRef reference direction in body frame (default Z-axis)
|
||||
*/
|
||||
Rot3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
|
||||
const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
Base(model, key), GravityFactor(nZ, bRef) {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
#else
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
#endif
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const;
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
|
||||
/** vector of errors */
|
||||
virtual Vector evaluateError(const Rot3& nRb, //
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H = OptionalNone) const {
|
||||
#else
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
#endif
|
||||
return attitudeError(nRb, H);
|
||||
}
|
||||
Unit3 nZ() const {
|
||||
return nZ_;
|
||||
}
|
||||
Unit3 bRef() const {
|
||||
return bRef_;
|
||||
}
|
||||
|
||||
private:
|
||||
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
/*ar & boost::serialization::make_nvp("NoiseModelFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & boost::serialization::make_nvp("GravityFactor",
|
||||
boost::serialization::base_object<GravityFactor>(*this));*/
|
||||
}
|
||||
#endif
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Version of GravityFactor for Pose3
|
||||
* @addtogroup Navigation
|
||||
*/
|
||||
class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
|
||||
public GravityFactor {
|
||||
|
||||
typedef NoiseModelFactor1<Pose3> Base;
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
|
||||
#else
|
||||
typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
|
||||
#endif
|
||||
/// Typedef to this class
|
||||
typedef Pose3GravityFactor This;
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
Pose3GravityFactor() {
|
||||
}
|
||||
|
||||
virtual ~Pose3GravityFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* @param key of the Pose3 variable that will be constrained
|
||||
* @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
|
||||
* @param model Gaussian noise model
|
||||
* @param bRef reference direction in body frame (default Z-axis)
|
||||
*/
|
||||
Pose3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
|
||||
const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
Base(model, key), GravityFactor(nZ, bRef) {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
#else
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
#endif
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const;
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
|
||||
/** vector of errors */
|
||||
virtual Vector evaluateError(const Pose3& nTb, //
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H = OptionalNone) const {
|
||||
#else
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
#endif
|
||||
Vector e = attitudeError(nTb.rotation(), H);
|
||||
if (H) {
|
||||
Matrix H23 = *H;
|
||||
*H = Matrix::Zero(2,6);
|
||||
H->block<2,3>(0,0) = H23;
|
||||
}
|
||||
return e;
|
||||
}
|
||||
Unit3 nZ() const {
|
||||
return nZ_;
|
||||
}
|
||||
Unit3 bRef() const {
|
||||
return bRef_;
|
||||
}
|
||||
|
||||
private:
|
||||
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
/*ar & boost::serialization::make_nvp("NoiseModelFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & boost::serialization::make_nvp("GravityFactor",
|
||||
boost::serialization::base_object<GravityFactor>(*this));*/
|
||||
}
|
||||
#endif
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
||||
@@ -43,7 +43,7 @@ public:
|
||||
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
|
||||
gtsam::Vector evaluateError(const VALUE& p,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H = OptionalNone) const {
|
||||
gtsam::OptionalMatrixType H = OptionalNone) const {
|
||||
#else
|
||||
boost::optional<gtsam::Matrix&> H = boost::none) const {
|
||||
#endif
|
||||
@@ -59,5 +59,5 @@ public:
|
||||
|
||||
};
|
||||
|
||||
} // namespace gtsamexamples
|
||||
} // namespace rtabmap
|
||||
|
||||
|
||||
@@ -43,7 +43,7 @@ public:
|
||||
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
|
||||
gtsam::Vector evaluateError(const gtsam::Pose3& p,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H = OptionalNone) const {
|
||||
gtsam::OptionalMatrixType H = OptionalNone) const {
|
||||
#else
|
||||
boost::optional<gtsam::Matrix&> H = boost::none) const {
|
||||
#endif
|
||||
@@ -55,7 +55,7 @@ public:
|
||||
}
|
||||
gtsam::Vector evaluateError(const gtsam::Point3& p,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H = OptionalNone) const {
|
||||
gtsam::OptionalMatrixType H = OptionalNone) const {
|
||||
#else
|
||||
boost::optional<gtsam::Matrix&> H = boost::none) const {
|
||||
#endif
|
||||
@@ -63,5 +63,5 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsamexamples
|
||||
} // namespace rtabmap
|
||||
|
||||
|
||||
Reference in New Issue
Block a user