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;