Merge pull request #1975 from mnissov/gps-lever-arm

Adding lever-arm to GPS factors
borglab · Jan 17, 2025 · 717e3be · 717e3be
2 parents 61f2fdd + d1106ae
commit 717e3be
Show file tree

Hide file tree

Showing 3 changed files with 559 additions and 5 deletions.
diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp
@@ -64,6 +64,69 @@ 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 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";
@@ -85,5 +148,67 @@ Vector GPSFactor2::evaluateError(const NavState& p,
 }
 
 //***************************************************************************
+void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const {
+  cout << s << "GPSFactor2Arm 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_;
+}
+
+//***************************************************************************
+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