Adding lever-arm to GPS factors

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

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,73 @@ 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_;
+  }
+
+};
+
+/**
+ * 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 +249,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

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) {