adding lever arm optional argument to gps factors

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_);
 }
 
 //***************************************************************************

gtsam/navigation/GPSFactor.h

@@ -39,6 +39,7 @@ private:
   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 @@ public:
   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 @@ public:
    * @param gpsIn measurement already in correct coordinates
    * @param model Gaussian noise model
    */
-  GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
+  GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) :
-      Base(model, key), nT_(gpsIn) {
+      Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) {
   }
 
   /// @return a deep copy of this factor
@@ -87,6 +88,10 @@ 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
@@ -122,6 +127,7 @@ private:
   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 @@ public:
   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) :
+  GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) :
-      Base(model, key), nT_(gpsIn) {
+      Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) {
   }
 
   /// @return a deep copy of this factor
@@ -164,6 +170,10 @@ public:
     return nT_;
   }
 
+  inline const Point3 & leverArm() const {
+    return B_t_BG_;
+  }
+
 private:
 
 #if GTSAM_ENABLE_BOOST_SERIALIZATION

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