EquivariantFilter and GroupAction#2197
Conversation
dellaert
left a comment
dellaert
left a comment
There was a problem hiding this comment.
Nice progress! Some initial comments.
…equire for template, change EqF to take in initial state, use traits for group
jenniferoum
force-pushed
the
feature/equivariant-filter
branch
from
73215df to
4ee2efe
Compare
|
Nice to see this one in gtsam 👍🏻 |
|
Thans @AlessandroFornasier !We're going to substantially rewrite and base on LieGroupEKF (or derived), we'll let you know :-) |
dellaert
left a comment
dellaert
left a comment
There was a problem hiding this comment.
Good meeting. Trying to give some more feedback here. The goal is whether we can get rid of Geometry altogether.
refactored to remove dependency on Geometry some comments
…actor Equivariant Filter Optimizations (?)
There was a problem hiding this comment.
Pull request overview
This PR templates the Equivariant Filter on Group and Manifold structures, reorganizing the ABC (Attitude-Bias-Calibration) system into a more modular and generic architecture. The changes separate geometry-specific definitions from filter-specific functions and provide a reusable framework for equivariant filtering.
Key Changes:
- Introduces generic
EquivariantFilterclass templated on manifold state and symmetry group - Moves ABC geometry definitions to
gtsam_unstable/geometry/ABC.h - Adds
GroupActionbase class for defining group actions on manifolds - Refactors
ManifoldEKFto expose reusable Kalman gain and Joseph update methods
Reviewed changes
Copilot reviewed 16 out of 17 changed files in this pull request and generated 12 comments.
Show a summary per file
| File | Description |
|---|---|
| gtsam_unstable/geometry/ABC.h | Defines ABC-specific geometry: State, Group, Symmetry, Lift, Input/Output actions |
| gtsam/navigation/EquivariantFilter.h | Generic equivariant filter implementation templated on manifold and symmetry |
| gtsam/base/GroupAction.h | CRTP base class and helpers for group actions (Orbit, Diffeomorphism, InducedVectorField) |
| gtsam/navigation/ManifoldEKF.h | Refactored to expose KalmanGain() and JosephUpdate() as reusable methods |
| gtsam/base/ProductLieGroup.h | Changed Jacobian typedefs from protected to public for external access |
| gtsam/geometry/Unit3.h/cpp | Added cross product methods with Jacobian support for Unit3 x Unit3/Point3 |
| gtsam_unstable/geometry/tests/testABC.cpp | Comprehensive test suite for ABC components |
| gtsam/navigation/tests/testEquivariantFilter.cpp | Tests generic filter with simple SO(3) attitude example |
| examples/AbcEquivariantFilterExample.cpp | Updated example using new API structure |
| examples/ABC_EQF.h, examples/ABC.h | Removed (functionality moved to library) |
Allow orbits/diffeomorphism if no Jacobian
dellaert
changed the title
|
@AlessandroFornasier want to take a new look? I think we successfully replicated your paper in an elegant framework now. |
scottiyio
left a comment
scottiyio
left a comment
There was a problem hiding this comment.
Just want to check on the notation of z - zhat
AlessandroFornasier
left a comment
AlessandroFornasier
left a comment
There was a problem hiding this comment.
Generally LGTM. Nice job! I am very happy to see this within GTSAM!
| * @return MatrixM The calculated error dynamics matrix A. | ||
| */ | ||
| template <typename Lift, typename InputOrbit> | ||
| MatrixM computeErrorDynamicsMatrix(const InputOrbit& psi_u) const { |
There was a problem hiding this comment.
Mentioning only for future work.
An EqF can still be designed even if a compatible action psi is not found. Current version seems to not support this case. Could be a nice addition in future.
In such case you would have something like the following:
//...
typename Symmetry::Orbit act_on_x(this->X_);
typename Symmetry::Orbit coords_act(act_on_x(traits<G>::Identity()));
coords_act(g_.inverse(), D_phi);
Lift lift_u_origin(u);
lift_u_origin(act_on_ref_(g_), &D_lift);
return D_phi * D_lift
gtsam/navigation/EquivariantFilter.h
Outdated
| * P = (I - K * H) * P * (I - K * H)^T + K * R * K^T | ||
| */ | ||
| template <typename VectorZ, typename MatrixZM, typename MatrixZ> | ||
| void updateInternal(const VectorZ& innovation, const MatrixZM& H, |
There was a problem hiding this comment.
Mentioning only for future work.
A reset step (also called curvature correction) could be implemented in the update and enabled under a flag eventually. See page 38-39 https://arxiv.org/pdf/2407.14297
| const Vector3 nu = wedge_d * transformed_y; | ||
|
|
||
| if (H) { | ||
| *H = measurementMatrixC<N>(d_, index_); |
There was a problem hiding this comment.
Just a question out of curiosity. Any plan to extend this to support measurementMatrixCStar ? Reference to equations 3.24-3.25 page 34 https://arxiv.org/pdf/2407.14297
|
One more thing, maybe |
|
Thanks a bunch, @scottiyio and @AlessandroFornasier ! I will keep some of these as future work (non equivariant input, and equivariant output), but will address the rest. In a meeting with Stephan I also simplified the abc::Group. |
dellaert
left a comment
dellaert
left a comment
There was a problem hiding this comment.
Thanks everyone! A team effort, and great reviews! I will merge now!
Overview
This PR templates the Equivariant Filter on Group and Manifold structures, reorganizing the ABC (Attitude-Bias-Calibration) system into a more modular and generic architecture. The changes separate geometry-specific definitions from filter-specific functions and provide a reusable framework for equivariant filtering.
Key Changes:
gtsam_unstable/geometry/ABC.hAbcEquivariantFilterExample.cppTesting