Renamed GroupEKF -> LieGroupEKF, moved invariant predicts

examples/GEKF_Rot3Example.cpp

@@ -20,7 +20,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/geometry/Rot3.h>
-#include <gtsam/navigation/GroupEKF.h>
+#include <gtsam/navigation/LieGroupEKF.h>
 
 #include <iostream>
 
@@ -53,7 +53,7 @@ int main() {
   // Initial estimate (identity) and covariance
   const Rot3 R0 = Rot3::RzRyRx(0.1, -0.2, 0.3);
   const Matrix3 P0 = Matrix3::Identity() * 0.1;
-  GroupEKF<Rot3> ekf(R0, P0);
+  LieGroupEKF<Rot3> ekf(R0, P0);
 
   // Timestep, process noise, measurement noise
   double dt = 0.1;

gtsam/navigation/InvariantEKF.h

@@ -13,10 +13,10 @@
   * @file    InvariantEKF.h
   * @brief   Left-Invariant Extended Kalman Filter implementation.
   *
-  * This file defines the InvariantEKF class template, inheriting from GroupEKF,
+  * This file defines the InvariantEKF class template, inheriting from LieGroupEKF,
   * which specifically implements the Left-Invariant EKF formulation. It restricts
   * prediction methods to only those based on group composition (state-independent
-  * motion models), hiding the state-dependent prediction variants from GroupEKF.
+  * motion models), hiding the state-dependent prediction variants from LieGroupEKF.
   *
   * @date    April 24, 2025
   * @authors Scott Baker, Matt Kielo, Frank Dellaert
@@ -24,7 +24,7 @@
 
 #pragma once
 
-#include <gtsam/navigation/GroupEKF.h> // Include the base class
+#include <gtsam/navigation/LieGroupEKF.h> // Include the base class
 
 namespace gtsam {
 
@@ -34,50 +34,54 @@ namespace gtsam {
    *
    * @tparam G Lie group type (must satisfy LieGroup concept).
    *
-   * This filter inherits from GroupEKF but restricts the prediction interface
+   * This filter inherits from LieGroupEKF but restricts the prediction interface
    * to only the left-invariant prediction methods:
    * 1. Prediction via group composition: `predict(const G& U, const Matrix& Q)`
    * 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Matrix& Q)`
    *
-   * The state-dependent prediction methods from GroupEKF are hidden.
+   * The state-dependent prediction methods from LieGroupEKF are hidden.
-   * The update step remains the same as in ManifoldEKF/GroupEKF.
+   * The update step remains the same as in ManifoldEKF/LieGroupEKF.
    */
   template <typename G>
-  class InvariantEKF : public GroupEKF<G> {
+  class InvariantEKF : public LieGroupEKF<G> {
   public:
-    using Base = GroupEKF<G>; ///< Base class type
+    using Base = LieGroupEKF<G>; ///< Base class type
     using TangentVector = typename Base::TangentVector; ///< Tangent vector type
     using MatrixN = typename Base::MatrixN; ///< Square matrix type for covariance etc.
+    using Jacobian = typename Base::Jacobian; ///< Jacobian matrix type specific to the group G
 
-    /// Constructor: forwards to GroupEKF constructor
+    /// Constructor: forwards to LieGroupEKF constructor
     InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {}
 
-    // --- Expose only the Invariant Prediction Methods ---
-
     /**
      * Predict step via group composition (Left-Invariant):
      *   X_{k+1} = X_k * U
      *   P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q
-     * Calls the base class implementation.
+     * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. This corresponds to
+     * F = Ad_{U^{-1}} in the base class predict method.
      *
      * @param U Lie group element representing the motion increment.
      * @param Q Process noise covariance in the tangent space (size nxn).
      */
     void predict(const G& U, const Matrix& Q) {
-      Base::predict(U, Q);
+      this->X_ = this->X_.compose(U);
+      // TODO(dellaert): traits<G>::AdjointMap should exist
+      Jacobian A = traits<G>::Inverse(U).AdjointMap();
+      this->P_ = A * this->P_ * A.transpose() + Q;
     }
 
     /**
      * Predict step via tangent control vector:
      *   U = Expmap(u * dt)
-     * Then calls predict(U, Q). Calls the base class implementation.
+     * Then calls predict(U, Q).
      *
      * @param u Tangent space control vector.
      * @param dt Time interval.
      * @param Q Process noise covariance matrix (size nxn).
      */
     void predict(const TangentVector& u, double dt, const Matrix& Q) {
-      Base::predict(u, dt, Q);
+      G U = traits<G>::Expmap(u * dt);
+      predict(U, Q); // Call the group composition predict
     }
 
   }; // InvariantEKF

gtsam/navigation/LieGroupEKF.h

@@ -10,13 +10,13 @@
  * -------------------------------------------------------------------------- */
 
  /**
-  * @file  GroupEKF.h
+  * @file  LieGroupEKF.h
   * @brief   Extended Kalman Filter derived class for Lie groups G.
   *
-  * This file defines the GroupEKF class template, inheriting from ManifoldEKF,
+  * This file defines the LieGroupEKF class template, inheriting from ManifoldEKF,
   * for performing EKF steps specifically on states residing in a Lie group.
-  * It provides predict methods utilizing group composition, tangent space
+  * It provides predict methods with state-dependent dynamics functions.
-  * controls (via exponential map), and state-dependent dynamics functions.
+  * Please use the InvariantEKF class for prediction via group composition.
   *
   * @date  April 24, 2025
   * @authors Scott Baker, Matt Kielo, Frank Dellaert
@@ -34,18 +34,18 @@
 namespace gtsam {
 
   /**
-   * @class GroupEKF
+   * @class LieGroupEKF
    * @brief Extended Kalman Filter on a Lie group G, derived from ManifoldEKF
    *
    * @tparam G Lie group type providing group operations and Expmap/AdjointMap.
    *       Must satisfy LieGroup concept (`gtsam::IsLieGroup<G>::value`).
    *
-   * This filter specializes ManifoldEKF for Lie groups, offering convenient
+   * This filter specializes ManifoldEKF for Lie groups, offering predict methods
-   * prediction methods based on group composition or dynamics functions defining
+   * with state-dependent dynamics functions.
-   * motion in the tangent space.
+   * Use the InvariantEKF class for prediction via group composition.
    */
   template <typename G>
-  class GroupEKF : public ManifoldEKF<G> {
+  class LieGroupEKF : public ManifoldEKF<G> {
   public:
     using Base = ManifoldEKF<G>; ///< Base class type
     static constexpr int n = Base::n; ///< Group dimension (tangent space dimension)
@@ -54,41 +54,10 @@ namespace gtsam {
     using Jacobian = Eigen::Matrix<double, n, n>; ///< Jacobian matrix type specific to the group G
 
     /// Constructor: initialize with state and covariance
-    GroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {
+    LieGroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {
       static_assert(IsLieGroup<G>::value, "Template parameter G must be a GTSAM Lie Group");
     }
 
-    /**
-     * Predict step via group composition (Left-Invariant):
-     *   X_{k+1} = X_k * U
-     *   P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q
-     * where Ad_{U^{-1}} is the Adjoint map of U^{-1}. This corresponds to
-     * F = Ad_{U^{-1}} in the base class predict method.
-     *
-     * @param U Lie group element representing the motion increment.
-     * @param Q Process noise covariance in the tangent space (size nxn).
-     */
-    void predict(const G& U, const Matrix& Q) {
-      G X_next = this->X_.compose(U);
-      // TODO(dellaert): traits<G>::AdjointMap should exist
-      Jacobian A = traits<G>::Inverse(U).AdjointMap(); // A = Adjoint(U.inverse())
-      Base::predict(X_next, A, Q); // Call base class predict
-    }
-
-    /**
-     * Predict step via tangent control vector:
-     *   U = Expmap(u * dt)
-     * Then calls predict(U, Q).
-     *
-     * @param u Tangent space control vector.
-     * @param dt Time interval.
-     * @param Q Process noise covariance matrix (size nxn).
-     */
-    void predict(const TangentVector& u, double dt, const Matrix& Q) {
-      G U = traits<G>::Expmap(u * dt);
-      predict(U, Q); // Call the group composition predict
-    }
-
     /**
      * SFINAE check for correctly typed state-dependent dynamics function.
      * Signature: TangentVector f(const G& X, OptionalJacobian<n, n> Df)
@@ -142,8 +111,8 @@ namespace gtsam {
     template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
     void predict(Dynamics&& f, double dt, const Matrix& Q) {
       Jacobian A;
-      G X_next = predictMean(std::forward<Dynamics>(f), dt, A);
+      this->X_ = predictMean(std::forward<Dynamics>(f), dt, A);
-      Base::predict(X_next, A, Q); // Call base class predict
+      this->P_ = A * this->P_ * A.transpose() + Q;
     }
 
     /**
@@ -191,6 +160,6 @@ namespace gtsam {
       return predict([&](const G& X, OptionalJacobian<n, n> Df) { return f(X, u, Df); }, dt, Q);
     }
 
-  }; // GroupEKF
+  }; // LieGroupEKF
 
 }  // namespace gtsam

gtsam/navigation/tests/testLieGroupEKF.cpp

@@ -9,7 +9,7 @@
 
  /**
   * @file testGroupEKF.cpp
-  * @brief Unit test for GroupEKF, as well as dynamics used in Rot3 example.
+  * @brief Unit test for LieGroupEKF, as well as dynamics used in Rot3 example.
   * @date April 26, 2025
   * @authors Scott Baker, Matt Kielo, Frank Dellaert
   */
@@ -18,12 +18,12 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Rot3.h>
-#include <gtsam/navigation/GroupEKF.h>
+#include <gtsam/navigation/LieGroupEKF.h>
 #include <gtsam/navigation/NavState.h>
 
 using namespace gtsam;
 
-// Duplicate the dynamics function in GroupEKF_Rot3Example
+// Duplicate the dynamics function in GEKF_Rot3Example
 namespace exampleSO3 {
   static constexpr double k = 0.5;
   Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
@@ -63,12 +63,12 @@ TEST(GroupeEKF, PredictNumericState) {
 
   // Analytic Jacobian
   Matrix3 actualH;
-  GroupEKF<Rot3> ekf0(R0, P0);
+  LieGroupEKF<Rot3> ekf0(R0, P0);
   ekf0.predictMean(exampleSO3::dynamics, dt, actualH);
 
   // wrap predict into a state->state functor (mapping on SO(3))
   auto g = [&](const Rot3& R) -> Rot3 {
-    GroupEKF<Rot3> ekf(R, P0);
+    LieGroupEKF<Rot3> ekf(R, P0);
     return ekf.predictMean(exampleSO3::dynamics, dt);
     };
 
@@ -93,12 +93,12 @@ TEST(GroupeEKF, StateAndControl) {
 
   // Analytic Jacobian
   Matrix3 actualH;
-  GroupEKF<Rot3> ekf0(R0, P0);
+  LieGroupEKF<Rot3> ekf0(R0, P0);
   ekf0.predictMean(f, dummy_u, dt, actualH);
 
   // wrap predict into a state->state functor (mapping on SO(3))
   auto g = [&](const Rot3& R) -> Rot3 {
-    GroupEKF<Rot3> ekf(R, P0);
+    LieGroupEKF<Rot3> ekf(R, P0);
     return ekf.predictMean(f, dummy_u, dt, Q);
     };