diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 47de385ef..e4767009b 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -64,6 +64,37 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, return make_pair(nTb, nV); } + +//*************************************************************************** +void GPSFactorArm::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n"; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + cout << " Lever arm: " << bL_.transpose() << "\n"; + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactorArm::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(bL_, e->bL_, tol); +} + +//*************************************************************************** +Vector GPSFactorArm::evaluateError(const Pose3& p, + OptionalMatrixType H) const { + const Matrix3 nRb = p.rotation().matrix(); + if (H) { + H->resize(3, 6); + + H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_); + H->block<3, 3>(0, 3) = nRb; + } + + return p.translation() + nRb * bL_ - nT_; +} + //*************************************************************************** void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; @@ -85,5 +116,34 @@ Vector GPSFactor2::evaluateError(const NavState& p, } //*************************************************************************** +void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n"; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + cout << " Lever arm: " << bL_.transpose() << "\n"; + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactor2Arm::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(bL_, e->bL_, tol); +} + +//*************************************************************************** +Vector GPSFactor2Arm::evaluateError(const NavState& p, + OptionalMatrixType H) const { + const Matrix3 nRb = p.attitude().matrix(); + if (H) { + H->resize(3, 9); + + H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_); + H->block<3, 3>(0, 3) = nRb; + H->block<3, 3>(0, 6).setZero(); + } + + return p.position() + nRb * bL_ - nT_; +} }/// namespace gtsam diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 6af184360..58641f8e7 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -24,7 +24,10 @@ namespace gtsam { /** - * Prior on position in a Cartesian frame. + * Prior on position in a Cartesian frame, assuming position prior is in body + * frame. + * If there exists a non-zero lever arm between body frame and GPS + * antenna, instead use GPSFactorArm. * Possibilities include: * ENU: East-North-Up navigation frame at some local origin * NED: North-East-Down navigation frame at some local origin @@ -112,7 +115,81 @@ private: }; /** - * Version of GPSFactor for NavState + * Version of GPSFactor (for Pose3) with lever arm between GPS and Body frame. + * Because the actual location of the antenna depends on both position and + * attitude, providing a lever arm makes components of the attitude observable + * and accounts for position measurement vs state discrepancies while turning. + * @ingroup navigation + */ +class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN { + +private: + + typedef NoiseModelFactorN Base; + + Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D + ///< position of the GPS antenna in the body frame + +public: + + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /// Typedef to this class + typedef GPSFactorArm This; + + /// default constructor - only use for serialization + GPSFactorArm():nT_(0, 0, 0), bL_(0, 0, 0) {} + + ~GPSFactorArm() override {} + + /// Constructor from a measurement in a Cartesian frame. + GPSFactorArm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsIn), bL_(leverArm) { + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + + /// vector of errors + Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override; + + inline const Point3 & measurementIn() const { + return nT_; + } + + inline const Point3 & leverArm() const { + return bL_; + } + + /** + * Convenience function to estimate state at time t, given two GPS + * readings (in local NED Cartesian frame) bracketing t + * Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector. + */ + static std::pair EstimateState(double t1, const Point3& NED1, + double t2, const Point3& NED2, double timestamp); + +}; + +/** + * Version of GPSFactor for NavState, assuming zero lever arm between body frame + * and GPS. If there exists a non-zero lever arm between body frame and GPS + * antenna, instead use GPSFactor2Arm. * @ingroup navigation */ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN { @@ -180,4 +257,68 @@ private: #endif }; +/** + * Version of GPSFactor2 with lever arm between GPS and Body frame. + * Because the actual location of the antenna depends on both position and + * attitude, providing a lever arm makes components of the attitude observable + * and accounts for position measurement vs state discrepancies while turning. + * @ingroup navigation + */ +class GTSAM_EXPORT GPSFactor2Arm: public NoiseModelFactorN { + +private: + + typedef NoiseModelFactorN Base; + + Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D + ///< position of the GPS antenna in the body frame + +public: + + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /// Typedef to this class + typedef GPSFactor2Arm This; + + /// default constructor - only use for serialization + GPSFactor2Arm():nT_(0, 0, 0), bL_(0, 0, 0) {} + + ~GPSFactor2Arm() override {} + + /// Constructor from a measurement in a Cartesian frame. + GPSFactor2Arm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsIn), bL_(leverArm) { + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + + /// vector of errors + Vector evaluateError(const NavState& p, OptionalMatrixType H) const override; + + inline const Point3 & measurementIn() const { + return nT_; + } + + inline const Point3 & leverArm() const { + return bL_; + } + +}; + } /// namespace gtsam diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index cecfbeaad..7925f0aeb 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -44,7 +44,11 @@ const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84); // Dekalb-Peachtree Airport runway 2L -const double lat = 33.87071, lon = -84.30482, h = 274; +const double lat = 33.87071, lon = -84.30482, h = 274;\ + +// Random lever arm +const Point3 leverArm(0.1, 0.2, 0.3); + } // ************************************************************************* @@ -79,6 +83,37 @@ TEST( GPSFactor, Constructor ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +// ************************************************************************* +TEST( GPSFactorArm, Constructor ) { + using namespace example; + + // From lat-lon to geocentric + double E, N, U; + origin_ENU.Forward(lat, lon, h, E, N, U); + + // Factor + Key key(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + GPSFactorArm factor(key, Point3(E, N, U), leverArm, model); + + // Create a linearization point at zero error + const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 np = Point3(E, N, U) - nRb * leverArm; + Pose3 T(nRb, np); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative11( + [&factor](const Pose3& T) { return factor.evaluateError(T); }, T); + + // Use the factor to calculate the derivative + Matrix actualH; + factor.evaluateError(T, actualH); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + // ************************************************************************* TEST( GPSFactor2, Constructor ) { using namespace example; @@ -108,6 +143,37 @@ TEST( GPSFactor2, Constructor ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +// ************************************************************************* +TEST( GPSFactor2Arm, Constructor ) { + using namespace example; + + // From lat-lon to geocentric + double E, N, U; + origin_ENU.Forward(lat, lon, h, E, N, U); + + // Factor + Key key(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + GPSFactor2Arm factor(key, Point3(E, N, U), leverArm, model); + + // Create a linearization point at zero error + const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 np = Point3(E, N, U) - nRb * leverArm; + NavState T(nRb, np, Vector3::Zero()); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative11( + [&factor](const NavState& T) { return factor.evaluateError(T); }, T); + + // Use the factor to calculate the derivative + Matrix actualH; + factor.evaluateError(T, actualH); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + //*************************************************************************** TEST(GPSData, init) {