diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 47de385ef..ff1f9923b 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -26,20 +26,31 @@ namespace gtsam { void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? "" : s + " ") << "GPSFactor on " << keyFormatter(key()) << "\n"; - cout << " GPS measurement: " << nT_ << "\n"; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + cout << " Lever arm: " << B_t_BG_.transpose() << "\n"; noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(nT_, e->nT_, tol) && + traits::Equals(B_t_BG_, e->B_t_BG_, tol); } //*************************************************************************** Vector GPSFactor::evaluateError(const Pose3& p, OptionalMatrixType H) const { - return p.translation(H) -nT_; + const Matrix3 rot = p.rotation().matrix(); + if (H) { + H->resize(3, 6); + + H->block<3, 3>(0, 0) = -rot * skewSymmetric(B_t_BG_); + H->block<3, 3>(0, 3) = rot; + } + + return p.translation() - (nT_ - rot * B_t_BG_); } //*************************************************************************** @@ -67,7 +78,8 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, //*************************************************************************** void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; - cout << " GPS measurement: " << nT_.transpose() << endl; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + cout << " Lever arm: " << B_t_BG_.transpose() << "\n"; noiseModel_->print(" noise model: "); } @@ -75,13 +87,23 @@ void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); return e != nullptr && Base::equals(*e, tol) && - traits::Equals(nT_, e->nT_, tol); + traits::Equals(nT_, e->nT_, tol) && + traits::Equals(B_t_BG_, e->B_t_BG_, tol); } //*************************************************************************** Vector GPSFactor2::evaluateError(const NavState& p, OptionalMatrixType H) const { - return p.position(H) -nT_; + const Matrix3 rot = p.attitude().matrix(); + if (H) { + H->resize(3, 9); + + H->block<3, 3>(0, 0) = -rot * skewSymmetric(B_t_BG_); + H->block<3, 3>(0, 3) = rot; + H->block<3, 3>(0, 6).setZero(); + } + + return p.position() - (nT_ - rot * B_t_BG_); } //*************************************************************************** diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 6af184360..e954d719b 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -39,6 +39,7 @@ private: typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame public: @@ -52,7 +53,7 @@ public: typedef GPSFactor This; /** default constructor - only use for serialization */ - GPSFactor(): nT_(0, 0, 0) {} + GPSFactor(): nT_(0, 0, 0), B_t_BG_(0, 0, 0) {} ~GPSFactor() override {} @@ -63,8 +64,8 @@ public: * @param gpsIn measurement already in correct coordinates * @param model Gaussian noise model */ - GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : - Base(model, key), nT_(gpsIn) { + GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) : + Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) { } /// @return a deep copy of this factor @@ -87,6 +88,10 @@ public: return nT_; } + inline const Point3 & leverArm() const { + return B_t_BG_; + } + /** * Convenience function to estimate state at time t, given two GPS * readings (in local NED Cartesian frame) bracketing t @@ -122,6 +127,7 @@ private: typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame public: @@ -135,13 +141,13 @@ public: typedef GPSFactor2 This; /// default constructor - only use for serialization - GPSFactor2():nT_(0, 0, 0) {} + GPSFactor2():nT_(0, 0, 0), B_t_BG_(0, 0, 0) {} ~GPSFactor2() override {} /// Constructor from a measurement in a Cartesian frame. - GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : - Base(model, key), nT_(gpsIn) { + GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) : + Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) { } /// @return a deep copy of this factor @@ -164,6 +170,10 @@ public: return nT_; } + inline const Point3 & leverArm() const { + return B_t_BG_; + } + private: #if GTSAM_ENABLE_BOOST_SERIALIZATION diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index cecfbeaad..2972d0fd6 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -45,6 +45,9 @@ LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84); // Dekalb-Peachtree Airport runway 2L const double lat = 33.87071, lon = -84.30482, h = 274; + +// Random lever arm +const Point3 leverArm(0.1, 0.2, 0.3); } // ************************************************************************* @@ -61,10 +64,12 @@ TEST( GPSFactor, Constructor ) { // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); - GPSFactor factor(key, Point3(E, N, U), model); + GPSFactor factor(key, Point3(E, N, U), model, leverArm); // Create a linearization point at zero error - Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); + const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 p = Point3(E, N, U) - rot * leverArm; + Pose3 T(rot, p); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives @@ -90,10 +95,12 @@ TEST( GPSFactor2, Constructor ) { // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); - GPSFactor2 factor(key, Point3(E, N, U), model); + GPSFactor2 factor(key, Point3(E, N, U), model, leverArm); // Create a linearization point at zero error - NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero()); + const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 p = Point3(E, N, U) - rot * leverArm; + NavState T(rot, p, Vector3::Zero()); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives