diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 47de385ef2..f01a16d902 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -64,6 +64,69 @@ 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 GPSFactorArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactorArmCalib on " << keyFormatter(key()) << "\n"; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactorArmCalib::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); +} + +//*************************************************************************** +Vector GPSFactorArmCalib::evaluateError(const Pose3& p, const Point3& bL, + OptionalMatrixType H1, OptionalMatrixType H2) const { + const Matrix3 nRb = p.rotation().matrix(); + if (H1) { + H1->resize(3, 6); + + H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL); + H1->block<3, 3>(0, 3) = nRb; + } + if (H2){ + H2->resize(3, 3); + *H2 = 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 +148,67 @@ Vector GPSFactor2::evaluateError(const NavState& p, } //*************************************************************************** +void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactor2Arm 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_; +} + +//*************************************************************************** +void GPSFactor2ArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactor2ArmCalib on " << keyFormatter(key()) << "\n"; + cout << " GPS measurement: " << nT_.transpose() << "\n"; + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactor2ArmCalib::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); +} + +//*************************************************************************** +Vector GPSFactor2ArmCalib::evaluateError(const NavState& p, const Point3& bL, + OptionalMatrixType H1, OptionalMatrixType H2) const { + const Matrix3 nRb = p.attitude().matrix(); + if (H1) { + H1->resize(3, 9); + + H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL); + H1->block<3, 3>(0, 3) = nRb; + H1->block<3, 3>(0, 6).setZero(); + } + if (H2){ + H2->resize(3, 3); + *H2 = nRb; + } + + return p.position() + nRb * bL - nT_; +} }/// namespace gtsam diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 6af1843603..4ef7c9794d 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -25,6 +25,8 @@ namespace gtsam { /** * Prior on position in a Cartesian 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 @@ -38,7 +40,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN { typedef NoiseModelFactorN Base; - Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame) public: @@ -83,6 +85,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN { /// vector of errors Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override; + /// return the measurement, in the navigation frame inline const Point3 & measurementIn() const { return nT_; } @@ -112,7 +115,146 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN { }; /** - * 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 (navigation frame) + 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. + * @param key key of the Pose3 variable related to this measurement + * @param gpsIn gps measurement, in Cartesian navigation frame + * @param leverArm translation from the body frame origin to the gps antenna, in body frame + * @param model Gaussian noise model + */ + 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; + + /// return the measurement, in the navigation frame + inline const Point3 & measurementIn() const { + return nT_; + } + + /// return the lever arm, a position in the body frame + inline const Point3 & leverArm() const { + return bL_; + } + +}; + +/** + * Version of GPSFactorArm (for Pose3) with unknown 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 GPSFactorArmCalib: public NoiseModelFactorN { + +private: + + typedef NoiseModelFactorN Base; + + Point3 nT_; ///< Position measurement in cartesian coordinates (navigation 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 GPSFactorArmCalib This; + + /// default constructor - only use for serialization + GPSFactorArmCalib() : nT_(0, 0, 0) {} + + ~GPSFactorArmCalib() override {} + + /** Constructor from a measurement in a Cartesian frame. + * @param key1 key of the Pose3 variable related to this measurement + * @param key2 key of the Point3 variable related to the lever arm + * @param gpsIn gps measurement, in Cartesian navigation frame + * @param leverArm translation from the body frame origin to the gps antenna, in body frame + * @param model Gaussian noise model + */ + GPSFactorArmCalib(Key key1, Key key2, const Point3& gpsIn, const SharedNoiseModel& model) : + Base(model, key1, key2), nT_(gpsIn) { + } + + /// @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, const Point3& bL, OptionalMatrixType H1, + OptionalMatrixType H2) const override; + + /// return the measurement, in the navigation frame + inline const Point3 & measurementIn() const { + return nT_; + } +}; + +/** + * 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 { @@ -121,7 +263,7 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN { typedef NoiseModelFactorN Base; - Point3 nT_; ///< Position measurement in cartesian coordinates + Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame) public: @@ -139,7 +281,11 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN { ~GPSFactor2() override {} - /// Constructor from a measurement in a Cartesian frame. + /** Constructor from a measurement in a Cartesian frame. + * @param key key of the NavState variable related to this measurement + * @param gpsIn gps measurement, in Cartesian navigation frame + * @param model Gaussian noise model + */ GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : Base(model, key), nT_(gpsIn) { } @@ -160,6 +306,7 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN { /// vector of errors Vector evaluateError(const NavState& p, OptionalMatrixType H) const override; + /// return the measurement, in the navigation frame inline const Point3 & measurementIn() const { return nT_; } @@ -180,4 +327,140 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN { #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 (navigation frame) + 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. + * @param key key of the NavState variable related to this measurement + * @param gpsIn gps measurement, in Cartesian navigation frame + * @param leverArm translation from the body frame origin to the gps antenna, in body frame + * @param model noise model for the factor's residual + */ + 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; + + /// return the measurement, in the navigation frame + inline const Point3 & measurementIn() const { + return nT_; + } + + /// return the lever arm, a position in the body frame + inline const Point3 & leverArm() const { + return bL_; + } + +}; + +/** + * Version of GPSFactor2Arm for an unknown 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 GPSFactor2ArmCalib: public NoiseModelFactorN { + +private: + + typedef NoiseModelFactorN Base; + + Point3 nT_; ///< Position measurement in cartesian coordinates (navigation 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 GPSFactor2ArmCalib This; + + /// default constructor - only use for serialization + GPSFactor2ArmCalib():nT_(0, 0, 0) {} + + ~GPSFactor2ArmCalib() override {} + + /** Constructor from a measurement in a Cartesian frame. + * @param key1 key of the NavState variable related to this measurement + * @param key2 key of the Point3 variable related to the lever arm + * @param gpsIn gps measurement, in Cartesian navigation frame + * @param model Gaussian noise model + */ + GPSFactor2ArmCalib(Key key1, Key key2, const Point3& gpsIn, const SharedNoiseModel& model) : + Base(model, key1, key2), nT_(gpsIn) { + } + + /// @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, const Point3& bL, + OptionalMatrixType H1, + OptionalMatrixType H2) const override; + + /// return the measurement, in the navigation frame + inline const Point3 & measurementIn() const { + return nT_; + } +}; + } /// namespace gtsam diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index cecfbeaad5..f240e5dbfe 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,77 @@ 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( GPSFactorArmCalib, Constructor ) { + using namespace example; + + // From lat-lon to geocentric + double E, N, U; + origin_ENU.Forward(lat, lon, h, E, N, U); + + // Factor + Key key1(1), key2(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + GPSFactorArmCalib factor(key1, key2, Point3(E, N, U), 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, leverArm),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + [&factor, &leverArm](const Pose3& pose_arg) { + return factor.evaluateError(pose_arg, leverArm); + }, + T); + Matrix expectedH2 = numericalDerivative11( + [&factor, &T](const Point3& point_arg) { + return factor.evaluateError(T, point_arg); + }, + leverArm); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2; + factor.evaluateError(T, leverArm, actualH1, actualH2); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); +} + // ************************************************************************* TEST( GPSFactor2, Constructor ) { using namespace example; @@ -108,6 +183,77 @@ 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( GPSFactor2ArmCalib, Constructor ) { + using namespace example; + + // From lat-lon to geocentric + double E, N, U; + origin_ENU.Forward(lat, lon, h, E, N, U); + + // Factor + Key key1(1), key2(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + GPSFactor2ArmCalib factor(key1, key2, Point3(E, N, U), 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, leverArm),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + [&factor, &leverArm](const NavState& nav_arg) { + return factor.evaluateError(nav_arg, leverArm); + }, + T); + Matrix expectedH2 = numericalDerivative11( + [&factor, &T](const Point3& point_arg) { + return factor.evaluateError(T, point_arg); + }, + leverArm); + + // Use the factor to calculate the derivative + Matrix actualH1, actualH2; + factor.evaluateError(T, leverArm, actualH1, actualH2); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); +} + //*************************************************************************** TEST(GPSData, init) {