From af1e6e34e6d5c7df470bec301f6b203c8d2beace Mon Sep 17 00:00:00 2001 From: morten Date: Wed, 15 Jan 2025 11:54:41 +0100 Subject: [PATCH] reverting changes to existing factors --- gtsam/navigation/GPSFactor.cpp | 34 +++++------------------- gtsam/navigation/GPSFactor.h | 24 +++++------------ gtsam/navigation/navigation.i | 8 ++---- gtsam/navigation/tests/testGPSFactor.cpp | 15 +++-------- 4 files changed, 18 insertions(+), 63 deletions(-) diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index ff1f9923b..47de385ef 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -26,31 +26,20 @@ 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_.transpose() << "\n"; - cout << " Lever arm: " << B_t_BG_.transpose() << "\n"; + cout << " GPS measurement: " << nT_ << "\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) && - traits::Equals(B_t_BG_, e->B_t_BG_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** Vector GPSFactor::evaluateError(const Pose3& p, OptionalMatrixType H) const { - 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_); + return p.translation(H) -nT_; } //*************************************************************************** @@ -78,8 +67,7 @@ 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() << "\n"; - cout << " Lever arm: " << B_t_BG_.transpose() << "\n"; + cout << " GPS measurement: " << nT_.transpose() << endl; noiseModel_->print(" noise model: "); } @@ -87,23 +75,13 @@ 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(B_t_BG_, e->B_t_BG_, tol); + traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** Vector GPSFactor2::evaluateError(const NavState& p, OptionalMatrixType H) const { - 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_); + return p.position(H) -nT_; } //*************************************************************************** diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index b180911ae..6af184360 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -39,7 +39,6 @@ private: typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates - Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame public: @@ -53,7 +52,7 @@ public: typedef GPSFactor This; /** default constructor - only use for serialization */ - GPSFactor(): nT_(0, 0, 0), B_t_BG_(0, 0, 0) {} + GPSFactor(): nT_(0, 0, 0) {} ~GPSFactor() override {} @@ -64,8 +63,8 @@ public: * @param gpsIn measurement already in correct coordinates * @param model Gaussian noise model */ - GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) : - Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) { + GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsIn) { } /// @return a deep copy of this factor @@ -88,10 +87,6 @@ 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 @@ -112,7 +107,6 @@ private: & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(nT_); - ar & BOOST_SERIALIZATION_NVP(B_t_BG_); } #endif }; @@ -128,7 +122,6 @@ private: typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates - Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame public: @@ -142,13 +135,13 @@ public: typedef GPSFactor2 This; /// default constructor - only use for serialization - GPSFactor2():nT_(0, 0, 0), B_t_BG_(0, 0, 0) {} + GPSFactor2():nT_(0, 0, 0) {} ~GPSFactor2() override {} /// Constructor from a measurement in a Cartesian frame. - GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) : - Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) { + GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsIn) { } /// @return a deep copy of this factor @@ -171,10 +164,6 @@ public: return nT_; } - inline const Point3 & leverArm() const { - return B_t_BG_; - } - private: #if GTSAM_ENABLE_BOOST_SERIALIZATION @@ -187,7 +176,6 @@ private: & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(nT_); - ar & BOOST_SERIALIZATION_NVP(B_t_BG_); } #endif }; diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 7300f87ba..ceeab3b35 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -329,8 +329,7 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { #include virtual class GPSFactor : gtsam::NonlinearFactor{ GPSFactor(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model, - const gtsam::Point3& leverArm); + const gtsam::noiseModel::Base* model); // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = @@ -339,7 +338,6 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ // Standard Interface gtsam::Point3 measurementIn() const; - gtsam::Point3 leverArm() const; // enable serialization functionality void serialize() const; @@ -347,8 +345,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ virtual class GPSFactor2 : gtsam::NonlinearFactor { GPSFactor2(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model, - const gtsam::Point3& leverArm); + const gtsam::noiseModel::Base* model); // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = @@ -357,7 +354,6 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Standard Interface gtsam::Point3 measurementIn() const; - gtsam::Point3 leverArm() const; // enable serialization functionality void serialize() const; diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 2972d0fd6..cecfbeaad 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -45,9 +45,6 @@ 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); } // ************************************************************************* @@ -64,12 +61,10 @@ TEST( GPSFactor, Constructor ) { // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); - GPSFactor factor(key, Point3(E, N, U), model, leverArm); + GPSFactor factor(key, Point3(E, N, U), model); // Create a linearization point at zero error - const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45); - const Point3 p = Point3(E, N, U) - rot * leverArm; - Pose3 T(rot, p); + Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives @@ -95,12 +90,10 @@ TEST( GPSFactor2, Constructor ) { // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); - GPSFactor2 factor(key, Point3(E, N, U), model, leverArm); + GPSFactor2 factor(key, Point3(E, N, U), model); // Create a linearization point at zero error - 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()); + NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero()); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives