12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Renamed GroupEKF -> LieGroupEKF, moved invariant predicts

release/4.3a0
Frank Dellaert 2025-05-07 09:55:45 -04:00
parent 3bcc5563eb
commit ec8c762bb9
4 changed files with 41 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/GEKF_Rot3Example.cpp
View File

@ -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;

34
gtsam/navigation/InvariantEKF.h
View File

@ -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 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 <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

57
gtsam/navigation/GroupEKF.h → gtsam/navigation/LieGroupEKF.h
View File

@ -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<G>::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 <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);
Base::predict(X_next, A, Q); // Call base class predict
this->X_ = predictMean(std::forward<Dynamics>(f), dt, A);
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

14
gtsam/navigation/tests/testGroupEKF.cpp → gtsam/navigation/tests/testLieGroupEKF.cpp
View File

@ -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);
};