From 935246549459b34f805fe9f97d1151a69660d375 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 26 Apr 2025 12:14:12 -0400
Subject: [PATCH] More symmetric templating

---
 examples/LIEKF_NavstateExample.cpp |  4 ++--
 gtsam/nonlinear/LIEKF.h            | 24 +++++++++++++-----------
 2 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp
index 22639acb1..a99e91398 100644
--- a/examples/LIEKF_NavstateExample.cpp
+++ b/examples/LIEKF_NavstateExample.cpp
@@ -89,7 +89,7 @@ int main() {
   cout << "P0: " << ekf.covariance() << endl;
 
   // First prediction stage
-  ekf.predict<6>(dynamics, imu1, dt, Q);
+  ekf.predict(dynamics, imu1, dt, Q);
   cout << "\nFirst Prediction:\n";
   cout << "X: " << ekf.state() << endl;
   cout << "P: " << ekf.covariance() << endl;
@@ -101,7 +101,7 @@ int main() {
   cout << "P: " << ekf.covariance() << endl;
 
   // Second prediction stage
-  ekf.predict<6>(dynamics, imu2, dt, Q);
+  ekf.predict(dynamics, imu2, dt, Q);
   cout << "\nSecond Prediction:\n";
   cout << "X: " << ekf.state() << endl;
   cout << "P: " << ekf.covariance() << endl;
diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h
index 93fe5c8b5..3d71337fa 100644
--- a/gtsam/nonlinear/LIEKF.h
+++ b/gtsam/nonlinear/LIEKF.h
@@ -84,10 +84,13 @@ class LIEKF {
   }
 
   /**
-   * @brief Prediction stage with a control vector u and a time interval dt.
+   * @brief Prediction stage with a tangent vector xi and a time interval dt.
    * @param u Control vector element
    * @param dt Time interval
    * @param Q Process noise covariance matrix.
+   *
+   * @note Use this if your dynamics does not depend on the state, e.g.
+   * predict(f(u), dt, q) where u is a control input
    */
   void predict(const Vector& u, double dt, const Matrix& Q) {
     predict(G::Expmap(u * dt), Q);
@@ -95,28 +98,27 @@ class LIEKF {
 
   /**
    * @brief Prediction stage with a dynamics function that calculates the
-   * tangent vector xi in the tangent space.
-   * @tparam _p The dimension of the control vector.
-   * @tparam Dynamics : (G, VectorP, OptionalJacobian<n,n>) -> TangentVector
-   * @param f Dynamics function that depends on state and control vector
-   * @param u Control vector element
+   * tangent vector xi that *depends on the state*.
+   * @tparam Control The control input type
+   * @tparam Dynamics : (G, Control, OptionalJacobian<n,n>) -> TangentVector
+   * @param f Dynamics function that depends on state and control input
+   * @param u Control input
    * @param dt Time interval
    * @param Q Process noise covariance matrix.
    */
-  template <size_t _p, typename Dynamics>
-  void predict(Dynamics&& f, const Eigen::Matrix<double, _p, 1>& u, double dt,
-               const Matrix& Q) {
+  template <typename Control, typename Dynamics>
+  void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {
     typename G::Jacobian F;
     const typename G::TangentVector xi = f(X, u, F);
     G U = G::Expmap(xi * dt);
-    auto A = U.inverse().AdjointMap() * F;
+    auto A = U.inverse().AdjointMap() * F;  // chain rule for compose and f
     X = X.compose(U);
     P = A * P * A.transpose() + Q;
   }
 
   /**
    * @brief Update stage using a measurement and measurement covariance.
-   * @tparam Measurement
+   * @tparam Measurement The measurement output type
    * @tparam Prediction : (G, OptionalJacobian<m,n>) -> Measurement
    * @param z Measurement
    * @param R Measurement noise covariance matrix.