diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 4a6b35117..f01a16d90 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -95,6 +95,38 @@ Vector GPSFactorArm::evaluateError(const Pose3& p, 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"; @@ -146,4 +178,37 @@ Vector GPSFactor2Arm::evaluateError(const NavState& p, 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 e043281ce..4ef7c9794 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -185,6 +185,72 @@ public: }; +/** + * 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 @@ -332,4 +398,69 @@ public: }; +/** + * 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 7925f0aeb..f240e5dbf 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -114,6 +114,46 @@ TEST( GPSFactorArm, Constructor ) { 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; @@ -174,6 +214,46 @@ TEST( GPSFactor2Arm, Constructor ) { 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) {