From 5254b4f0b1fba5389ade67586c0f9e03394570e1 Mon Sep 17 00:00:00 2001
From: morten <morten.c.nissov@gmail.com>
Date: Mon, 13 Jan 2025 16:55:40 +0100
Subject: [PATCH 01/10] adding lever arm optional argument to gps factors

---
 gtsam/navigation/GPSFactor.cpp           | 34 +++++++++++++++++++-----
 gtsam/navigation/GPSFactor.h             | 22 ++++++++++-----
 gtsam/navigation/tests/testGPSFactor.cpp | 15 ++++++++---
 3 files changed, 55 insertions(+), 16 deletions(-)

diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp
index 47de385ef2..ff1f9923be 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<const This*>(&expected);
-  return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
+  return e != nullptr && Base::equals(*e, tol) &&
+         traits<Point3>::Equals(nT_, e->nT_, tol) &&
+         traits<Point3>::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<Pose3, Vector3> 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<const This*>(&expected);
   return e != nullptr && Base::equals(*e, tol) &&
-         traits<Point3>::Equals(nT_, e->nT_, tol);
+         traits<Point3>::Equals(nT_, e->nT_, tol) &&
+         traits<Point3>::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 6af1843603..e954d719b1 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -39,6 +39,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
   typedef NoiseModelFactorN<Pose3> Base;
 
   Point3 nT_; ///< Position measurement in cartesian coordinates
+  Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame
 
 public:
 
@@ -52,7 +53,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
   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 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
    * @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 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
     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 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
   typedef NoiseModelFactorN<NavState> Base;
 
   Point3 nT_; ///< Position measurement in cartesian coordinates
+  Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame
 
 public:
 
@@ -135,13 +141,13 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
   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 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
     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 cecfbeaad5..2972d0fd61 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

From 57b8f6b158e4fb633080443a0d4e0f8715831ab7 Mon Sep 17 00:00:00 2001
From: morten <morten.c.nissov@gmail.com>
Date: Tue, 14 Jan 2025 12:09:10 +0100
Subject: [PATCH 02/10] fix GPSFactor serialization

---
 gtsam/navigation/GPSFactor.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h
index e954d719b1..b180911ae9 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -112,6 +112,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
         & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(nT_);
+    ar & BOOST_SERIALIZATION_NVP(B_t_BG_);
   }
 #endif
 };
@@ -186,6 +187,7 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
         & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(nT_);
+    ar & BOOST_SERIALIZATION_NVP(B_t_BG_);
   }
 #endif
 };

From 7efcb468f501e046a335ab471b245e0896c4f785 Mon Sep 17 00:00:00 2001
From: morten <morten.c.nissov@gmail.com>
Date: Tue, 14 Jan 2025 12:09:27 +0100
Subject: [PATCH 03/10] update GPSFactor interface

---
 gtsam/navigation/navigation.i | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i
index ceeab3b355..7300f87ba8 100644
--- a/gtsam/navigation/navigation.i
+++ b/gtsam/navigation/navigation.i
@@ -329,7 +329,8 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
 #include <gtsam/navigation/GPSFactor.h>
 virtual class GPSFactor : gtsam::NonlinearFactor{
   GPSFactor(size_t key, const gtsam::Point3& gpsIn,
-            const gtsam::noiseModel::Base* model);
+            const gtsam::noiseModel::Base* model,
+            const gtsam::Point3& leverArm);
 
   // Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
@@ -338,6 +339,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
 
   // Standard Interface
   gtsam::Point3 measurementIn() const;
+  gtsam::Point3 leverArm() const;
 
   // enable serialization functionality
   void serialize() const;
@@ -345,7 +347,8 @@ 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::noiseModel::Base* model,
+            const gtsam::Point3& leverArm);
 
   // Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
@@ -354,6 +357,7 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
 
   // Standard Interface
   gtsam::Point3 measurementIn() const;
+  gtsam::Point3 leverArm() const;
 
   // enable serialization functionality
   void serialize() const;

From af1e6e34e6d5c7df470bec301f6b203c8d2beace Mon Sep 17 00:00:00 2001
From: morten <morten.c.nissov@gmail.com>
Date: Wed, 15 Jan 2025 11:54:41 +0100
Subject: [PATCH 04/10] 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 ff1f9923be..47de385ef2 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<const This*>(&expected);
-  return e != nullptr && Base::equals(*e, tol) &&
-         traits<Point3>::Equals(nT_, e->nT_, tol) &&
-         traits<Point3>::Equals(B_t_BG_, e->B_t_BG_, tol);
+  return e != nullptr && Base::equals(*e, tol) && traits<Point3>::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<Pose3, Vector3> 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<const This*>(&expected);
   return e != nullptr && Base::equals(*e, tol) &&
-         traits<Point3>::Equals(nT_, e->nT_, tol) &&
-         traits<Point3>::Equals(B_t_BG_, e->B_t_BG_, tol);
+         traits<Point3>::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 b180911ae9..6af1843603 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -39,7 +39,6 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
   typedef NoiseModelFactorN<Pose3> Base;
 
   Point3 nT_; ///< Position measurement in cartesian coordinates
-  Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame
 
 public:
 
@@ -53,7 +52,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
   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 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
    * @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 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
     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 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
         & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(nT_);
-    ar & BOOST_SERIALIZATION_NVP(B_t_BG_);
   }
 #endif
 };
@@ -128,7 +122,6 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
   typedef NoiseModelFactorN<NavState> Base;
 
   Point3 nT_; ///< Position measurement in cartesian coordinates
-  Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame
 
 public:
 
@@ -142,13 +135,13 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
   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 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
     return nT_;
   }
 
-  inline const Point3 & leverArm() const {
-    return B_t_BG_;
-  }
-
 private:
 
 #if GTSAM_ENABLE_BOOST_SERIALIZATION
@@ -187,7 +176,6 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
         & boost::serialization::make_nvp("NoiseModelFactor1",
             boost::serialization::base_object<Base>(*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 7300f87ba8..ceeab3b355 100644
--- a/gtsam/navigation/navigation.i
+++ b/gtsam/navigation/navigation.i
@@ -329,8 +329,7 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
 #include <gtsam/navigation/GPSFactor.h>
 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 2972d0fd61..cecfbeaad5 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

From b81593601a1c1c341cbf31561722be17302c108b Mon Sep 17 00:00:00 2001
From: morten <morten.c.nissov@gmail.com>
Date: Wed, 15 Jan 2025 13:31:38 +0100
Subject: [PATCH 05/10] creating GPSFactorArm and GPSFactor2Arm w/ lever arm
 arguments

---
 gtsam/navigation/GPSFactor.cpp           |  60 ++++++++++
 gtsam/navigation/GPSFactor.h             | 145 ++++++++++++++++++++++-
 gtsam/navigation/tests/testGPSFactor.cpp |  68 ++++++++++-
 3 files changed, 270 insertions(+), 3 deletions(-)

diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp
index 47de385ef2..e4767009b8 100644
--- a/gtsam/navigation/GPSFactor.cpp
+++ b/gtsam/navigation/GPSFactor.cpp
@@ -64,6 +64,37 @@ pair<Pose3, Vector3> 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<const This*>(&expected);
+  return e != nullptr && Base::equals(*e, tol) &&
+         traits<Point3>::Equals(nT_, e->nT_, tol) &&
+         traits<Point3>::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<const This*>(&expected);
+  return e != nullptr && Base::equals(*e, tol) &&
+         traits<Point3>::Equals(nT_, e->nT_, tol) &&
+         traits<Point3>::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 6af1843603..58641f8e7d 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 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
 };
 
 /**
- * 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<Pose3> {
+
+private:
+
+  typedef NoiseModelFactorN<Pose3> 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<GPSFactorArm> 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>(
+        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<Pose3, Vector3> 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<NavState> {
@@ -180,4 +257,68 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
 #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<NavState> {
+
+private:
+
+  typedef NoiseModelFactorN<NavState> 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<GPSFactor2Arm> 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>(
+        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 cecfbeaad5..7925f0aeb1 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<Vector, Pose3>(
+      [&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<Vector, NavState>(
+      [&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) {
 

From 94e96ab001a682657e04f7e2524d67335fcb2421 Mon Sep 17 00:00:00 2001
From: morten <morten.c.nissov@gmail.com>
Date: Wed, 15 Jan 2025 13:38:08 +0100
Subject: [PATCH 06/10] remove EstimateState from GPSFactorArm

---
 gtsam/navigation/GPSFactor.h | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h
index 58641f8e7d..75249e42e3 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -176,14 +176,6 @@ class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN<Pose3> {
     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<Pose3, Vector3> EstimateState(double t1, const Point3& NED1,
-      double t2, const Point3& NED2, double timestamp);
-
 };
 
 /**

From 4737f0d554040ac111c735a5782fc2b6ce47fb4c Mon Sep 17 00:00:00 2001
From: morten <morten.c.nissov@gmail.com>
Date: Thu, 16 Jan 2025 17:57:29 +0100
Subject: [PATCH 07/10] improving comments

---
 gtsam/navigation/GPSFactor.h | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h
index 75249e42e3..d911122e80 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -24,8 +24,7 @@
 namespace gtsam {
 
 /**
- * Prior on position in a Cartesian frame, assuming position prior is in body
- * frame.
+ * 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:
@@ -41,7 +40,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
 
   typedef NoiseModelFactorN<Pose3> Base;
 
-  Point3 nT_; ///< Position measurement in cartesian coordinates
+  Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
 
 public:
 
@@ -127,7 +126,7 @@ class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN<Pose3> {
 
   typedef NoiseModelFactorN<Pose3> Base;
 
-  Point3 nT_;  ///< Position measurement in cartesian coordinates
+  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
 
@@ -190,7 +189,7 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
 
   typedef NoiseModelFactorN<NavState> Base;
 
-  Point3 nT_; ///< Position measurement in cartesian coordinates
+  Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
 
 public:
 
@@ -262,7 +261,7 @@ class GTSAM_EXPORT GPSFactor2Arm: public NoiseModelFactorN<NavState> {
 
   typedef NoiseModelFactorN<NavState> Base;
 
-  Point3 nT_;  ///< Position measurement in cartesian coordinates
+  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
 

From b5349a74f5db644a7928c8998049e666a77a27ba Mon Sep 17 00:00:00 2001
From: morten <morten.c.nissov@gmail.com>
Date: Thu, 16 Jan 2025 18:12:07 +0100
Subject: [PATCH 08/10] improved documentation, params and getters

---
 gtsam/navigation/GPSFactor.h | 26 +++++++++++++++++++++++---
 1 file changed, 23 insertions(+), 3 deletions(-)

diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h
index d911122e80..e043281cec 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -85,6 +85,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
   /// 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_;
   }
@@ -146,7 +147,12 @@ class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN<Pose3> {
 
   ~GPSFactorArm() override {}
 
-  /// Constructor from a measurement in a Cartesian frame.
+  /** 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) {
   }
@@ -167,10 +173,12 @@ class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN<Pose3> {
   /// 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_;
   }
@@ -207,7 +215,11 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
 
   ~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) {
   }
@@ -228,6 +240,7 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
   /// 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_;
   }
@@ -281,7 +294,12 @@ class GTSAM_EXPORT GPSFactor2Arm: public NoiseModelFactorN<NavState> {
 
   ~GPSFactor2Arm() 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 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) {
   }
@@ -302,10 +320,12 @@ class GTSAM_EXPORT GPSFactor2Arm: public NoiseModelFactorN<NavState> {
   /// 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_;
   }

From 0c13038ed5dfe73668dd0ee5bd27a64a61169801 Mon Sep 17 00:00:00 2001
From: morten <morten.c.nissov@gmail.com>
Date: Thu, 16 Jan 2025 21:26:47 +0100
Subject: [PATCH 09/10] typo in print for GPSFactor2Arm

---
 gtsam/navigation/GPSFactor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp
index e4767009b8..4a6b351172 100644
--- a/gtsam/navigation/GPSFactor.cpp
+++ b/gtsam/navigation/GPSFactor.cpp
@@ -117,7 +117,7 @@ Vector GPSFactor2::evaluateError(const NavState& p,
 
 //***************************************************************************
 void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const {
-  cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n";
+  cout << s << "GPSFactor2Arm on " << keyFormatter(key()) << "\n";
   cout << "  GPS measurement: " << nT_.transpose() << "\n";
   cout << "  Lever arm: " << bL_.transpose() << "\n";
   noiseModel_->print("  noise model: ");

From d1106ae4a36627dfa9926c6255dc985a547070a7 Mon Sep 17 00:00:00 2001
From: morten <morten.c.nissov@gmail.com>
Date: Thu, 16 Jan 2025 21:27:42 +0100
Subject: [PATCH 10/10] adding GPSFactors with lever arm as state

---
 gtsam/navigation/GPSFactor.cpp           |  65 +++++++++++
 gtsam/navigation/GPSFactor.h             | 131 +++++++++++++++++++++++
 gtsam/navigation/tests/testGPSFactor.cpp |  80 ++++++++++++++
 3 files changed, 276 insertions(+)

diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp
index 4a6b351172..f01a16d902 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<const This*>(&expected);
+  return e != nullptr && Base::equals(*e, tol) &&
+         traits<Point3>::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<const This*>(&expected);
+  return e != nullptr && Base::equals(*e, tol) &&
+         traits<Point3>::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 e043281cec..4ef7c9794d 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -185,6 +185,72 @@ class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN<Pose3> {
 
 };
 
+/**
+ * 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<Pose3, Point3> {
+
+private:
+
+  typedef NoiseModelFactorN<Pose3, Point3> 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<GPSFactorArmCalib> 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>(
+        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 @@ class GTSAM_EXPORT GPSFactor2Arm: public NoiseModelFactorN<NavState> {
 
 };
 
+/**
+ * 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<NavState, Point3> {
+
+private:
+
+  typedef NoiseModelFactorN<NavState, Point3> 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<GPSFactor2ArmCalib> 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>(
+        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 7925f0aeb1..f240e5dbfe 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<Vector, Pose3>(
+      [&factor, &leverArm](const Pose3& pose_arg) {
+        return factor.evaluateError(pose_arg, leverArm);
+      },
+      T);
+  Matrix expectedH2 = numericalDerivative11<Vector, Point3>(
+      [&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<Vector, NavState>(
+      [&factor, &leverArm](const NavState& nav_arg) {
+        return factor.evaluateError(nav_arg, leverArm);
+      },
+      T);
+  Matrix expectedH2 = numericalDerivative11<Vector, Point3>(
+      [&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) {