Template EqF on Group and Manifold #2197

jenniferoum · 2025-07-10T04:28:42Z

Updates to template equivariant filter on group and manifold

Also template on geometry structure to allow functions to be called in EquivariantFilter.h
Reorganize files:
- ABC.h contains geometry specific definitions
- EquivariantFilter.h contains fitler specific functions
- Renamed ABC_EQF_Demo to AbbcEquivariantFilterExample.cpp

dellaert

Nice progress! Some initial comments.

gtsam_unstable/geometry/ABC.h

examples/ABC.h

examples/EquivariantFilter.h

…equire for template, change EqF to take in initial state, use traits for group

gtsam/navigation/EquivariantFilter.h

dellaert · 2025-08-29T17:47:37Z

examples/EquivariantFilter.h

+  Matrix Q = Geometry::processNoise(u);  // Replaces blockDiag(...)
+
+  X_hat = traits<G>::Compose(X_hat, traits<G>::Expmap(L * dt));
+  Sigma = Phi * Sigma * Phi.transpose() + Bt * Q * Bt.transpose() * dt;


This is already done in the ManifoldEKF, specialized to groups in the LieGroupEKF, which is the main reason to derive form that class.

dellaert · 2025-08-29T17:48:44Z

examples/EquivariantFilter.h

+  Vector3 delta_vec = Rot3::Hat(y.d.unitVector()) * action_result;
+  Matrix Dt = Geometry::outputMatrixDt(y.cal_idx, X_hat);
+
+  Matrix S = Ct * Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose();


You'll also find this somewhere in the existing hierarchy.

AlessandroFornasier · 2025-09-12T09:47:32Z

Nice to see this one in gtsam 👍🏻

dellaert · 2025-09-12T20:52:23Z

Thans @AlessandroFornasier !We're going to substantially rewrite and base on LieGroupEKF (or derived), we'll let you know :-)

dellaert

Good meeting. Trying to give some more feedback here. The goal is whether we can get rid of Geometry altogether.

dellaert · 2025-10-24T21:19:55Z

gtsam_unstable/geometry/ABC.h

-
-  /// Initialize the symmetry group G
-  G(const Rot3& A = Rot3::Identity(), const Matrix3& a = Matrix3::Zero(),
+  static constexpr int numSensors = N;


Should also be small n if we are to match the paper.

dellaert · 2025-10-24T21:22:23Z

gtsam_unstable/geometry/ABC.h

+
+  // Adjoint matrix of this group element (for SE(3) or similar)
+  Eigen::Matrix<double, dimension, dimension> AdjointMap() const {
+    // TODO: implement properly for your group structure.


It would be nice to implement for your group, or at least figure out the math, as in the invariant filter the A matrix is just $Ad_{U^{-1}}$. Might that be the case as well here, with U the “lifted” dynamics?

dellaert · 2025-10-24T21:27:43Z

gtsam_unstable/geometry/ABC.h

 */
 template <size_t N>
-Input velocityAction(const G<N>& X, const Input& u) {
+Input velocityAction(const Group<N>& X, const Input& u) {


The Input type is mixing two things I believe: a mathematical object, $(\omega,0)$ where the zeros are that “virtual” input. And then a data structure that is used to store noise parameters and what not. I think the code will be a lot more clear if you separate those concerns, rename Input to InputData, and then redefine Input as simply

Using Input = Vector6

Or even just use Vector6.

dellaert · 2025-10-24T21:29:36Z

gtsam_unstable/geometry/ABC.h

-  return gtsam::numericalDerivative11<Vector, G<N>>(
-      [&xi](const G<N>& g) { return xi.localCoordinates(g * xi); },
-      gtsam::traits<G<N>>::Identity());
+  return gtsam::numericalDerivative11<Vector, Group<N>>(


If this is only used in tests move it there.

dellaert · 2025-10-24T21:34:29Z

gtsam/navigation/EquivariantFilter.h

+#pragma once
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Rot3.h>


These includes seem out of place when M and G are generic.

dellaert · 2025-10-24T21:36:14Z

gtsam/navigation/EquivariantFilter.h

+  void update(const typename Geometry::Measurement& y);
+
+  static constexpr int DOF = gtsam::traits<G>::dimension;
+  static constexpr int n_cal = Geometry::n_cal;


Also out of place in a generic filter.

dellaert · 2025-10-24T21:41:41Z

gtsam_unstable/geometry/ABC.h

+   * @return Linearized state matrix
+   * Uses Matrix zero and Identity functions
+   */
+  static Matrix stateMatrixA(const GType& X_hat, const Input& u) {


Is this actually used in the filter?

dellaert · 2025-10-24T21:48:21Z

gtsam/navigation/EquivariantFilter.h

+  Matrix Q   = Geometry::processNoise(u);
+
+
+  this->X_ = traits<G>::Compose(this->X_, traits<G>::Expmap(L * dt));


This makes me think $\Phi = Ad_{U^{-1}}$ with $U=L dt$. If so we can then kill it from Geometry - and I bet this holds for any equivariant filter that updates the state this way. Look at the InvariantEKF.

dellaert · 2025-10-24T21:50:27Z

gtsam/navigation/EquivariantFilter.h

+  Vector Delta = InnovationLift * K * delta_vec;
+  this->X_ = traits<G>::Compose(traits<G>::Expmap(Delta), this->X_);
+  this->P_ = (Matrix::Identity(DOF, DOF) - K * Ct) * this->P_;
+}


Try to see whether you can do this update by constructing the parameters of update in the base class. It will be informative.

dellaert · 2025-10-24T21:54:11Z

gtsam_unstable/geometry/ABC.h

+
+/// Input struct for the Biased Attitude System
+
+struct Input {


I would split this file up: ABC.h is pure geometry. All the other data structures (Input, etc) should be moved to the example - there should be zero “infra” code in this geometry code.

dellaert reviewed Jul 10, 2025

View reviewed changes

jenniferoum added 2 commits July 30, 2025 09:30

Template EqF on Group and Manifold

eb95739

Address EqF template comments, add compile time check for functions r…

4ee2efe

…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 July 30, 2025 17:20

Remove abc_eqf_lib from EquivariantFilter.h

7e12b62

dellaert reviewed Aug 29, 2025

View reviewed changes

jenniferoum added 5 commits September 26, 2025 09:29

Set up EqF to inherit from LieGroupEKF

11a2459

Move Eqf files and add unit test for ABC.h

742c4e5

Fix AbcEqF include paths

c7da938

Add filter test

70c723c

Fix naming conventions, add prediction test

4c1f40d

dellaert reviewed Oct 24, 2025

View reviewed changes

addressed input/inputtype disparity, small-n

deb979d

		Matrix Q = Geometry::processNoise(u);


		this->X_ = traits<G>::Compose(this->X_, traits<G>::Expmap(L * dt));


		/// Input struct for the Biased Attitude System

		struct Input {

Template EqF on Group and Manifold #2197

Are you sure you want to change the base?

Template EqF on Group and Manifold #2197

Conversation

jenniferoum commented Jul 10, 2025

Uh oh!

dellaert left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

AlessandroFornasier commented Sep 12, 2025

Uh oh!

dellaert commented Sep 12, 2025

Uh oh!

dellaert left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants