diff --git a/examples/GEKF_Rot3Example.cpp b/examples/GEKF_Rot3Example.cpp index d931b7909..d81e01d54 100644 --- a/examples/GEKF_Rot3Example.cpp +++ b/examples/GEKF_Rot3Example.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include @@ -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 ekf(R0, P0); + LieGroupEKF ekf(R0, P0); // Timestep, process noise, measurement noise double dt = 0.1; diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 1d39981d7..82a3f8fec 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/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 // Include the base class +#include // 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 update step remains the same as in ManifoldEKF/GroupEKF. + * The state-dependent prediction methods from LieGroupEKF are hidden. + * The update step remains the same as in ManifoldEKF/LieGroupEKF. */ template - class InvariantEKF : public GroupEKF { + class InvariantEKF : public LieGroupEKF { public: - using Base = GroupEKF; ///< Base class type + using Base = LieGroupEKF; ///< 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::AdjointMap should exist + Jacobian A = traits::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::Expmap(u * dt); + predict(U, Q); // Call the group composition predict } }; // InvariantEKF diff --git a/gtsam/navigation/GroupEKF.h b/gtsam/navigation/LieGroupEKF.h similarity index 77% rename from gtsam/navigation/GroupEKF.h rename to gtsam/navigation/LieGroupEKF.h index f2f2990e6..fe16eaefe 100644 --- a/gtsam/navigation/GroupEKF.h +++ b/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 - * controls (via exponential map), and state-dependent dynamics functions. + * It provides predict methods with 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::value`). * - * This filter specializes ManifoldEKF for Lie groups, offering convenient - * prediction methods based on group composition or dynamics functions defining - * motion in the tangent space. + * This filter specializes ManifoldEKF for Lie groups, offering predict methods + * with state-dependent dynamics functions. + * Use the InvariantEKF class for prediction via group composition. */ template - class GroupEKF : public ManifoldEKF { + class LieGroupEKF : public ManifoldEKF { public: using Base = ManifoldEKF; ///< Base class type static constexpr int n = Base::n; ///< Group dimension (tangent space dimension) @@ -54,41 +54,10 @@ namespace gtsam { using Jacobian = Eigen::Matrix; ///< 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::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::AdjointMap should exist - Jacobian A = traits::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::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 Df) @@ -142,8 +111,8 @@ namespace gtsam { template > void predict(Dynamics&& f, double dt, const Matrix& Q) { Jacobian A; - G X_next = predictMean(std::forward(f), dt, A); - Base::predict(X_next, A, Q); // Call base class predict + this->X_ = predictMean(std::forward(f), dt, A); + this->P_ = A * this->P_ * A.transpose() + Q; } /** @@ -191,6 +160,6 @@ namespace gtsam { return predict([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, Q); } - }; // GroupEKF + }; // LieGroupEKF } // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/tests/testGroupEKF.cpp b/gtsam/navigation/tests/testLieGroupEKF.cpp similarity index 90% rename from gtsam/navigation/tests/testGroupEKF.cpp rename to gtsam/navigation/tests/testLieGroupEKF.cpp index 5ce7c477b..62a35473d 100644 --- a/gtsam/navigation/tests/testGroupEKF.cpp +++ b/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 #include #include -#include +#include #include 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 ekf0(R0, P0); + LieGroupEKF 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 ekf(R, P0); + LieGroupEKF ekf(R, P0); return ekf.predictMean(exampleSO3::dynamics, dt); }; @@ -93,12 +93,12 @@ TEST(GroupeEKF, StateAndControl) { // Analytic Jacobian Matrix3 actualH; - GroupEKF ekf0(R0, P0); + LieGroupEKF 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 ekf(R, P0); + LieGroupEKF ekf(R, P0); return ekf.predictMean(f, dummy_u, dt, Q); };