From 7169c127faa70f6105dce821f143b17c0987a302 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Feb 2014 19:05:23 -0500 Subject: [PATCH] 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