From d0ee9c662de832c620fe1d26b24b38c29741b574 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 17:44:58 -0500 Subject: [PATCH 1/2] Switched to Rot3 and added Pose3 version --- gtsam/navigation/AttitudeFactor.cpp | 35 +++++++- gtsam/navigation/AttitudeFactor.h | 86 +++++++++++++++++-- gtsam/navigation/tests/testAttitudeFactor.cpp | 36 +++++++- 3 files changed, 149 insertions(+), 8 deletions(-) diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index de8eaf3bf..7cbf3e860 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -39,7 +39,40 @@ bool AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { } //*************************************************************************** -Vector AttitudeFactor::evaluateError(const Pose3& p, +Vector AttitudeFactor::evaluateError(const Rot3& nRb, + boost::optional H) const { + if (H) { + Matrix D_nRef_R, D_e_nRef; + Sphere2 nRef = nRb.rotate(bRef_, D_nRef_R); + Vector e = nZ_.error(nRef, D_e_nRef); + H->resize(2, 3); + H->block < 2, 3 > (0, 0) = D_e_nRef * D_nRef_R; + return e; + } else { + Sphere2 nRef = nRb * bRef_; + return nZ_.error(nRef); + } +} + +//*************************************************************************** +void Pose3AttitudeFactor::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "Pose3AttitudeFactor 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 Pose3AttitudeFactor::equals(const NonlinearFactor& expected, + double tol) const { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) + && this->bRef_.equals(e->bRef_, tol); +} + +//*************************************************************************** +Vector Pose3AttitudeFactor::evaluateError(const Pose3& p, boost::optional H) const { const Rot3& nRb = p.rotation(); if (H) { diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index e622a7e37..7a87a9871 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file AttitudeFactor.h + * @file Pose3AttitudeFactor.h * @author Frank Dellaert * @brief Header file for Attitude factor * @date January 28, 2014 @@ -24,18 +24,18 @@ namespace gtsam { /** - * Prior on the attitude of a Pose3. + * Prior on the attitude of a Rot3. * Example: * - measurement is direction of gravity in body frame bF * - reference is direction of gravity in navigation frame nG * This factor will give zero error if nRb * bF == nG * @addtogroup Navigation */ -class AttitudeFactor: public NoiseModelFactor1 { +class AttitudeFactor: public NoiseModelFactor1 { private: - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactor1 Base; Sphere2 nZ_, bRef_; ///< Position measurement in @@ -54,6 +54,80 @@ public: virtual ~AttitudeFactor() { } + /** + * @brief Constructor + * @param key of the Rot3 variable that will be constrained + * @param nZ measured direction in navigation frame + * @param model Gaussian noise model + * @param bRef reference direction in body frame (default Z-axis) + */ + AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model, + const Sphere2& bRef = Sphere2(0, 0, 1)) : + Base(model, key), nZ_(nZ), bRef_(bRef) { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** implement functions needed for Testable */ + + /** 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; + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Rot3& p, + boost::optional H = boost::none) const; + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(nZ_); + ar & BOOST_SERIALIZATION_NVP(bRef_); + } +}; + +/** + * Version of AttitudeFactor for Pose3 (this should be easier) + * @addtogroup Navigation + */ +class Pose3AttitudeFactor: public NoiseModelFactor1 { + +private: + + typedef NoiseModelFactor1 Base; + + Sphere2 nZ_, bRef_; ///< Position measurement in + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Typedef to this class + typedef Pose3AttitudeFactor This; + + /** default constructor - only use for serialization */ + Pose3AttitudeFactor() { + } + + virtual ~Pose3AttitudeFactor() { + } + /** * @brief Constructor * @param key of the Pose3 variable that will be constrained @@ -61,8 +135,8 @@ public: * @param model Gaussian noise model * @param bRef reference direction in body frame (default Z-axis) */ - AttitudeFactor(Key key, const Sphere2& nZ, - const SharedNoiseModel& model, const Sphere2& bRef=Sphere2(0,0,1)) : + Pose3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model, + const Sphere2& bRef = Sphere2(0, 0, 1)) : Base(model, key), nZ_(nZ), bRef_(bRef) { } diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 7f8590875..0f74f6c4c 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -40,13 +40,47 @@ TEST( AttitudeFactor, Constructor ) { AttitudeFactor factor(key, nDown, model, bZ); EXPECT(assert_equal(factor0,factor,1e-5)); + // Create a linearization point at the zero-error point + Rot3 nRb; + EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative11( + boost::bind(&AttitudeFactor::evaluateError, &factor, _1, boost::none), + nRb); + + // Use the factor to calculate the derivative + Matrix actualH; + factor.evaluateError(nRb, actualH); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +// ************************************************************************* +TEST( Pose3AttitudeFactor, Constructor ) { + + // Example: pitch and roll of aircraft in an ENU Cartesian frame. + // If pitch and roll are zero for an aerospace frame, + // that means Z is pointing down, i.e., direction of Z = (0,0,-1) + Sphere2 bZ(0, 0, 1); // reference direction is body Z axis + Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement" + + // Factor + Key key(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); + Pose3AttitudeFactor factor0(key, nDown, model); + Pose3AttitudeFactor factor(key, nDown, model, bZ); + EXPECT(assert_equal(factor0,factor,1e-5)); + // Create a linearization point at the zero-error point Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0)); EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&AttitudeFactor::evaluateError, &factor, _1, boost::none), T); + boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1, + boost::none), T); // Use the factor to calculate the derivative Matrix actualH; From 7169c127faa70f6105dce821f143b17c0987a302 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 19:05:23 -0500 Subject: [PATCH 2/2] Created common base class --- gtsam/navigation/AttitudeFactor.cpp | 53 ++++------- gtsam/navigation/AttitudeFactor.h | 93 ++++++++++++------- gtsam/navigation/tests/testAttitudeFactor.cpp | 10 +- 3 files changed, 82 insertions(+), 74 deletions(-) diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 7cbf3e860..fd5e92883 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -23,23 +23,7 @@ using namespace std; namespace gtsam { //*************************************************************************** -void AttitudeFactor::print(const string& s, - const KeyFormatter& keyFormatter) const { - cout << s << "AttitudeFactor 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 AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { - const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) - && this->bRef_.equals(e->bRef_, tol); -} - -//*************************************************************************** -Vector AttitudeFactor::evaluateError(const Rot3& nRb, +Vector AttitudeFactor::attitudeError(const Rot3& nRb, boost::optional H) const { if (H) { Matrix D_nRef_R, D_e_nRef; @@ -54,6 +38,23 @@ Vector AttitudeFactor::evaluateError(const Rot3& nRb, } } +//*************************************************************************** +void Rot3AttitudeFactor::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "Rot3AttitudeFactor 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 Rot3AttitudeFactor::equals(const NonlinearFactor& expected, + double tol) const { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) + && this->bRef_.equals(e->bRef_, tol); +} + //*************************************************************************** void Pose3AttitudeFactor::print(const string& s, const KeyFormatter& keyFormatter) const { @@ -71,24 +72,6 @@ bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected, && this->bRef_.equals(e->bRef_, tol); } -//*************************************************************************** -Vector Pose3AttitudeFactor::evaluateError(const Pose3& p, - boost::optional H) const { - const Rot3& nRb = p.rotation(); - if (H) { - Matrix D_nRef_R, D_e_nRef; - Sphere2 nRef = nRb.rotate(bRef_, D_nRef_R); - Vector e = nZ_.error(nRef, D_e_nRef); - H->resize(2, 6); - H->block < 2, 3 > (0, 0) = D_e_nRef * D_nRef_R; - H->block < 2, 3 > (0, 3) << Matrix::Zero(2, 3); - return e; - } else { - Sphere2 nRef = nRb * bRef_; - return nZ_.error(nRef); - } -} - //*************************************************************************** }/// namespace gtsam diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 7a87a9871..5290fc1e5 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -24,34 +24,60 @@ namespace gtsam { /** - * Prior on the attitude of a Rot3. + * Base class for prior on attitude * Example: * - measurement is direction of gravity in body frame bF * - reference is direction of gravity in navigation frame nG * This factor will give zero error if nRb * bF == nG * @addtogroup Navigation */ -class AttitudeFactor: public NoiseModelFactor1 { +class AttitudeFactor { -private: +protected: - typedef NoiseModelFactor1 Base; - - Sphere2 nZ_, bRef_; ///< Position measurement in + const Sphere2 nZ_, bRef_; ///< Position measurement in public: - /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /// Typedef to this class - typedef AttitudeFactor This; - /** default constructor - only use for serialization */ AttitudeFactor() { } - virtual ~AttitudeFactor() { + /** + * @brief Constructor + * @param nZ measured direction in navigation frame + * @param bRef reference direction in body frame (default Z-axis) + */ + AttitudeFactor(const Sphere2& nZ, const Sphere2& bRef = Sphere2(0, 0, 1)) : + nZ_(nZ), bRef_(bRef) { + } + + /** vector of errors */ + Vector attitudeError(const Rot3& p, + boost::optional H = boost::none) const; +}; + +/** + * Version of AttitudeFactor for Rot3 + * @addtogroup Navigation + */ +class Rot3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { + + typedef NoiseModelFactor1 Base; + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Typedef to this class + typedef Rot3AttitudeFactor This; + + /** default constructor - only use for serialization */ + Rot3AttitudeFactor() { + } + + virtual ~Rot3AttitudeFactor() { } /** @@ -61,9 +87,9 @@ public: * @param model Gaussian noise model * @param bRef reference direction in body frame (default Z-axis) */ - AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model, + Rot3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model, const Sphere2& bRef = Sphere2(0, 0, 1)) : - Base(model, key), nZ_(nZ), bRef_(bRef) { + Base(model, key), AttitudeFactor(nZ, bRef) { } /// @return a deep copy of this factor @@ -72,8 +98,6 @@ public: gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - /** implement functions needed for Testable */ - /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -81,11 +105,11 @@ public: /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; - /** implement functions needed to derive from Factor */ - /** vector of errors */ - Vector evaluateError(const Rot3& p, - boost::optional H = boost::none) const; + virtual Vector evaluateError(const Rot3& nRb, // + boost::optional H = boost::none) const { + return attitudeError(nRb, H); + } private: @@ -102,17 +126,14 @@ private: }; /** - * Version of AttitudeFactor for Pose3 (this should be easier) + * Version of AttitudeFactor for Pose3 * @addtogroup Navigation */ -class Pose3AttitudeFactor: public NoiseModelFactor1 { - -private: +class Pose3AttitudeFactor: public NoiseModelFactor1, + public AttitudeFactor { typedef NoiseModelFactor1 Base; - Sphere2 nZ_, bRef_; ///< Position measurement in - public: /// shorthand for a smart pointer to a factor @@ -137,7 +158,7 @@ public: */ Pose3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model, const Sphere2& bRef = Sphere2(0, 0, 1)) : - Base(model, key), nZ_(nZ), bRef_(bRef) { + Base(model, key), AttitudeFactor(nZ, bRef) { } /// @return a deep copy of this factor @@ -146,8 +167,6 @@ public: gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - /** implement functions needed for Testable */ - /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -155,11 +174,17 @@ public: /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; - /** implement functions needed to derive from Factor */ - /** vector of errors */ - Vector evaluateError(const Pose3& p, - boost::optional H = boost::none) const; + virtual Vector evaluateError(const Pose3& nTb, // + boost::optional H = boost::none) const { + 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; + } private: diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 0f74f6c4c..9edb2bbf5 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -11,7 +11,7 @@ /** * @file testAttitudeFactor.cpp - * @brief Unit test for AttitudeFactor + * @brief Unit test for Rot3AttitudeFactor * @author Frank Dellaert * @date January 22, 2014 */ @@ -25,7 +25,7 @@ using namespace std; using namespace gtsam; // ************************************************************************* -TEST( AttitudeFactor, Constructor ) { +TEST( Rot3AttitudeFactor, Constructor ) { // Example: pitch and roll of aircraft in an ENU Cartesian frame. // If pitch and roll are zero for an aerospace frame, @@ -36,8 +36,8 @@ TEST( AttitudeFactor, Constructor ) { // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); - AttitudeFactor factor0(key, nDown, model); - AttitudeFactor factor(key, nDown, model, bZ); + Rot3AttitudeFactor factor0(key, nDown, model); + Rot3AttitudeFactor factor(key, nDown, model, bZ); EXPECT(assert_equal(factor0,factor,1e-5)); // Create a linearization point at the zero-error point @@ -46,7 +46,7 @@ TEST( AttitudeFactor, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&AttitudeFactor::evaluateError, &factor, _1, boost::none), + boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none), nRb); // Use the factor to calculate the derivative