diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index de8eaf3bf..fd5e92883 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -23,32 +23,14 @@ 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 Pose3& p, +Vector AttitudeFactor::attitudeError(const Rot3& nRb, 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->resize(2, 3); 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_; @@ -56,6 +38,40 @@ Vector AttitudeFactor::evaluateError(const Pose3& p, } } +//*************************************************************************** +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 { + 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); +} + //*************************************************************************** }/// namespace gtsam diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index e622a7e37..5290fc1e5 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,34 +24,129 @@ namespace gtsam { /** - * Prior on the attitude of a Pose3. + * 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() { + } + + /** + * @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) + */ + Rot3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model, + const Sphere2& bRef = Sphere2(0, 0, 1)) : + Base(model, key), AttitudeFactor(nZ, 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))); + } + + /** 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, // + boost::optional H = boost::none) const { + return attitudeError(nRb, H); + } + +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 + * @addtogroup Navigation + */ +class Pose3AttitudeFactor: 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 Pose3AttitudeFactor This; + + /** default constructor - only use for serialization */ + Pose3AttitudeFactor() { + } + + virtual ~Pose3AttitudeFactor() { } /** @@ -61,9 +156,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, const Sphere2& bRef=Sphere2(0,0,1)) : - Base(model, key), nZ_(nZ), bRef_(bRef) { + Pose3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model, + const Sphere2& bRef = Sphere2(0, 0, 1)) : + Base(model, key), AttitudeFactor(nZ, bRef) { } /// @return a deep copy of this factor @@ -72,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; @@ -81,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 7f8590875..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,41 @@ 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 + Rot3 nRb; + EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative11( + boost::bind(&Rot3AttitudeFactor::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 @@ -46,7 +79,8 @@ TEST( AttitudeFactor, Constructor ) { // 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;