Merge pull request #2373 from borglab/feature/reset

EKF Reset

Author: dellaert

.gitignore

@@ -31,3 +31,4 @@ doc/#*.lyx#
 .venv/
 conductor/
 .ccache/
+gtsam_unstable/timing/data/

gtsam/geometry/tests/testGal3.cpp

@@ -83,6 +83,27 @@ TEST(Gal3, ChartDerivatives) {
   CHECK_CHART_DERIVATIVES(kTestGal3_Lie1, kTestGal3_Lie2);
 }
 
+/* ************************************************************************* */
+TEST(Gal3, RetractJacobians) {
+  const Gal3 g = kTestGal3_Lie1;
+  const Vector10 v = (Vector10() << 0.01, -0.02, 0.03, 0.1, -0.05, 0.2, 0.4,
+                     -0.3, 0.2, 0.01)
+                        .finished();
+
+  Matrix actualH1, actualH2;
+  traits<Gal3>::Retract(g, v, &actualH1, &actualH2);
+
+  std::function<Gal3(const Gal3&, const Vector10&)> retract_proxy =
+      [](const Gal3& g_, const Vector10& v_) { return g_.retract(v_); };
+  Matrix expectedH1 =
+      numericalDerivative21<Gal3, Gal3, Vector10>(retract_proxy, g, v);
+  Matrix expectedH2 =
+      numericalDerivative22<Gal3, Gal3, Vector10>(retract_proxy, g, v);
+
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
+  EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
+}
+
 
 /* ************************************************************************* */
 TEST(Gal3, StaticConstructorsValue) {

gtsam/geometry/tests/testPose2.cpp

@@ -75,6 +75,20 @@ TEST(Pose2, retract) {
   EXPECT(assert_equal(expected, actual, 1e-5));
 }
 
+/* ************************************************************************* */
+TEST(Pose2, retractJacobian) {
+  Pose2 pose(M_PI / 2.0, Point2(1, 2));
+  Vector3 v(0.01, -0.015, 0.99);
+
+  Matrix3 actualH;
+  traits<Pose2>::Retract(pose, v, {}, &actualH);
+
+  auto retract_from_pose = [&](const Vector3& delta) { return pose.retract(delta); };
+  Matrix3 expectedH = numericalDerivative11<Pose2, Vector3, 3>(retract_from_pose, v, 1e-6);
+
+  EXPECT(assert_equal(expectedH, actualH, 1e-5));
+}
+
 /* ************************************************************************* */
 TEST(Pose2, expmap) {
   Pose2 pose(M_PI/2.0, Point2(1, 2));
@@ -997,4 +1011,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

gtsam/navigation/ManifoldEKF.h

@@ -135,7 +135,8 @@ class ManifoldEKF {
     return P_ * H.transpose() * S.inverse();
   }
 
-  /// Joseph-form covariance update using a precomputed gain.
+  /// Joseph-form covariance update in the current tangent space using a
+  /// precomputed gain.
   template <typename GainMatrix, typename HMatrix, typename RMatrix>
   void JosephUpdate(const GainMatrix& K, const HMatrix& H, const RMatrix& R) {
     Jacobian I_KH = I_ - K * H;
@@ -151,14 +152,17 @@ class ManifoldEKF {
    * @param H Jacobian of the measurement function h.
    * @param z Observed measurement.
    * @param R Measurement noise covariance.
+   * @param performReset If true (default), performs a reset (transport) after
+   * update; otherwise, just retracts the state.
    */
   template <typename Measurement>
   void update(
       const Measurement& prediction,
       const Eigen::Matrix<double, traits<Measurement>::dimension, Dim>& H,
       const Measurement& z,
       const Eigen::Matrix<double, traits<Measurement>::dimension,
-                          traits<Measurement>::dimension>& R) {
+                          traits<Measurement>::dimension>& R,
+      bool performReset = true) {
     static constexpr int MeasDim = traits<Measurement>::dimension;
 
     // Innovation: y = h(x_pred) - z. In tangent space: local(z, h(x_pred))
@@ -174,11 +178,14 @@ class ManifoldEKF {
     const TangentVector delta_xi =
         -K * innovation;  // delta_xi is Dim x 1 (or n_ x 1 if dynamic)
 
-    // Update state using retract: X_new = retract(X_old, delta_xi)
-    X_ = traits<M>::Retract(X_, delta_xi);
-
-    // Update covariance using Joseph form
+    // --- Update covariance in the tangent space at the current state
     this->JosephUpdate(K, H, R);
+
+    // Update state using retract/ transport or just retract
+    if (performReset)
+      reset(delta_xi);
+    else
+      X_ = traits<M>::Retract(X_, delta_xi);
   }
 
   /**
@@ -190,11 +197,13 @@ class ManifoldEKF {
    * @param h Measurement model function.
    * @param z Observed measurement.
    * @param R Measurement noise covariance.
+   * @param performReset If true (default), transport covariance after retract.
    */
   template <typename Measurement, typename MeasurementFunction>
   void update(MeasurementFunction&& h, const Measurement& z,
               const Eigen::Matrix<double, traits<Measurement>::dimension,
-                                  traits<Measurement>::dimension>& R) {
+                                  traits<Measurement>::dimension>& R,
+              bool performReset = true) {
     static_assert(IsManifold<Measurement>::value,
                   "Template parameter Measurement must be a GTSAM Manifold.");
 
@@ -203,17 +212,45 @@ class ManifoldEKF {
     Measurement prediction = h(X_, H);
 
     // Call the other update function
-    update<Measurement>(prediction, H, z, R);
+    update<Measurement>(prediction, H, z, R, performReset);
   }
 
-  /// Convenience bridge for wrappers: vector measurement update calling
-  /// update<Vector>. This overload exists to avoid templates in wrappers. It
-  /// validates sizes and forwards to the templated update with Measurement =
-  /// gtsam::Vector (dynamic size).
+  /**
+   * Convenience bridge for wrappers: vector measurement update calling
+   * update<Vector>. This overload exists to avoid templates in wrappers. It
+   * validates sizes and forwards to the templated update with Measurement =
+   * gtsam::Vector (dynamic size).
+   * @param prediction Predicted measurement vector.
+   * @param H Measurement Jacobian matrix.
+   * @param z Observed measurement vector.
+   * @param R Measurement noise covariance matrix.
+   * @param performReset If true (default), transport covariance after retract.
+   */
   void updateWithVector(const gtsam::Vector& prediction, const Matrix& H,
-                        const gtsam::Vector& z, const Matrix& R) {
+                        const gtsam::Vector& z, const Matrix& R,
+                        bool performReset = true) {
     validateInputs(prediction, H, z, R);
-    update<Vector>(prediction, H, z, R);
+    update<Vector>(prediction, H, z, R, performReset);
+  }
+
+  /**
+   * Reset step: retract the state by a tangent perturbation and, if available,
+   * transport the covariance from the old tangent space to the new tangent
+   * space.
+   *
+   * If the retract supports a Jacobian argument, we compute B and update
+   * P <- B P B^T. Otherwise, we leave the covariance unchanged.
+   */
+  void reset(const TangentVector& eta) {
+    if constexpr (HasRetractJacobian<M>::value) {
+      Jacobian B;
+      if constexpr (Dim == Eigen::Dynamic) B.resize(n_, n_);
+      X_ = traits<M>::Retract(X_, eta, &B);
+      P_ = B * P_ * B.transpose();
+    } else {
+      X_ = traits<M>::Retract(X_, eta);
+      // Covariance unchanged when Jacobian is not available.
+    }
   }
 
  protected:
@@ -243,6 +280,17 @@ class ManifoldEKF {
   Covariance P_;  ///< Covariance (Eigen::Matrix<double, Dim, Dim>).
   Jacobian I_;    ///< Identity matrix sized to the state dimension.
   int n_;         ///< Runtime tangent space dimension of M.
+
+ private:
+  // Detection helper: check if traits<T>::Retract(x, v, Jacobian*) is valid.
+  template <typename T, typename = void>
+  struct HasRetractJacobian : std::false_type {};
+  template <typename T>
+  struct HasRetractJacobian<
+      T, std::void_t<decltype(traits<T>::Retract(
+             std::declval<const T&>(),
+             std::declval<const typename traits<T>::TangentVector&>(),
+             (Jacobian*)nullptr))>> : std::true_type {};
 };
 
 }  // namespace gtsam

gtsam/navigation/navigation.i

@@ -551,6 +551,7 @@ virtual class Scenario {
   gtsam::Vector acceleration_n(double t) const;
   gtsam::Rot3 rotation(double t) const;
   gtsam::NavState navState(double t) const;
+  gtsam::Gal3 gal3(double t) const;
   gtsam::Vector velocity_b(double t) const;
   gtsam::Vector acceleration_b(double t) const;
 };
@@ -619,7 +620,7 @@ virtual class ManifoldEKF {
 
   // Only vector-based measurements are supported in wrapper
   void updateWithVector(const gtsam::Vector& prediction, const gtsam::Matrix& H,
-                        const gtsam::Vector& z, const gtsam::Matrix& R);
+                        const gtsam::Vector& z, const gtsam::Matrix& R, bool performReset = true);
 };
 
 #include <gtsam/navigation/LieGroupEKF.h>

gtsam/navigation/tests/testInvariantEKF.cpp

@@ -65,7 +65,7 @@ TEST(IEKF_Pose2, PredictUpdateSequence) {
   EXPECT(assert_equal(P1_expected, ekf.covariance(), 1e-9));
 
   // --- First Update ---
-  ekf.update(h_gps, z1, R);
+  ekf.update(h_gps, z1, R, false);
 
   // Calculate expected state and covariance (manual Kalman steps)
   Matrix H1;  // H = dh/dlocal(X) -> 2x3
@@ -97,7 +97,7 @@ TEST(IEKF_Pose2, PredictUpdateSequence) {
   EXPECT(assert_equal(P2_expected, ekf.covariance(), 1e-9));
 
   // --- Second Update ---
-  ekf.update(h_gps, z2, R);
+  ekf.update(h_gps, z2, R, false);
 
   // Calculate expected state and covariance (manual Kalman steps)
   Matrix H2;  // 2x3