EqVIO Filter Implementation by rohan-bansal · Pull Request #2475 · borglab/gtsam

rohan-bansal · 2026-03-22T21:48:58Z

Changelog:

Previous PR that introduced VIO group/manifold also implemented retract and localCoordinates Jacobians in ProductLiegroup-inl.h for dynamic landmarks. CI was failing due to initialized Jacobians here, I patch this here.
Added state/output matrices according to the inverse-depth coordinate suite in the paper. I use InvDepth because the paper proves it can adequately preserve the equivariant output linearization while remaining numerically stable for distant points, and it is the path of least resistance for making the example work. The other two coordinate suites (euclid/polar) are currently not implemented; support will be added for Polar in a future PR.
Added EqVIOFilter.h/cpp, extending from EquivariantFilter.h. Made some changes to the base class to support syncing/re-binding filter internals once landmarks are added/removed, since this is an event where the model dimension changes.
Added an EqVIOFilterExample.cpp that runs the filter on the first 10 seconds of the EuRoC MAV Vicon Room 1 dataset. I ran feature tracking independently and condensed input information into a CSV that is less than 1mb in size. The first 300 lines or so of this file are helpers to read the CSV and build the buffers; the main loop is below it.

Working example output below!

Events: 2202, IMU: 2001, vision frames: 201, vision features: 3030
Measurement noise variance (normalized): 1.77557e-05
Filter time: 1403715283.2621431
Landmarks: 19
Pose translation: -0.9407490307 -0.1008082292  0.1802625531
GT pose translation: -0.954631 -0.101702  0.179862
Velocity: -0.1197946952 -0.3369151421  0.1426375951
GT velocity: -0.120739 -0.314283  0.119599

rohan-bansal · 2026-03-25T00:53:17Z

Ready for review @dellaert :)

dellaert · 2026-03-26T15:35:39Z

I’m traveling so it might be a bit before I get to it.

Copilot

Pull request overview

Implements an EqVIO (equivariant VIO) filter on top of EquivariantFilter, adds the EqF linearization blocks using an inverse-depth landmark chart, updates symmetry/action utilities accordingly, and introduces tests + an example replay driver.

Changes:

Add EqVIOFilter (dynamic landmark management, propagate/correct pipeline) and corresponding unit tests.
Extend EqVIO symmetry utilities with inverse-depth EqF matrices (A/B/C) and innovation lifting for group updates.
Update base EquivariantFilter to support derived filters that need to reset/sync internal buffers when dimensions change.

Reviewed changes

Copilot reviewed 12 out of 13 changed files in this pull request and generated 9 comments.

Show a summary per file

File	Description
gtsam_unstable/navigation/tests/testEqVIOSymmetry.cpp	Updates symmetry tests and adds helper routines used by the new symmetry behavior.
gtsam_unstable/navigation/tests/testEqVIOFilter.cpp	Adds unit tests for initialization, propagation parity, vision update, and dynamic landmarks.
gtsam_unstable/navigation/EqVIOSymmetry.h	Adds/updates public EqVIO symmetry APIs + EqF matrices + innovation lift; renames action to `Symmetry`.
gtsam_unstable/navigation/EqVIOSymmetry.cpp	Implements inverse-depth chart conversions, EqF matrices, and innovation lift; refactors action helpers.
gtsam_unstable/navigation/EqVIOState.cpp	Minor cleanup + adds `gravityDir()` comment.
gtsam_unstable/navigation/EqVIOFilter.h	Introduces `EqVIOFilter` API and parameter struct.
gtsam_unstable/navigation/EqVIOFilter.cpp	Implements filter logic: propagation, correction, outlier rejection, and dynamic landmark resize/sync.
gtsam_unstable/navigation/EqVIOCommon.h	Adds brief doc comments for group typedefs.
gtsam_unstable/examples/EqVIOFilterExample.cpp	Adds a replay example that consumes a CSV event stream and prints terminal stats.
gtsam/nonlinear/doc/PriorFactor.ipynb	Fixes markdown link formatting and cell string formatting.
gtsam/navigation/EquivariantFilter.h	Adds protected reset/sync hooks and a vector update overload with custom innovation lift; fixes identity sizing for dynamic groups.
gtsam/base/ProductLieGroup-inl.h	Initializes Jacobians to avoid uninitialized usage in dynamic settings.

gtsam_unstable/navigation/EqVIOSymmetry.cpp

gtsam_unstable/examples/EqVIOFilterExample.cpp

gtsam_unstable/navigation/EqVIOSymmetry.h

gtsam_unstable/navigation/EqVIOState.cpp

gtsam_unstable/navigation/EqVIOSymmetry.cpp

gtsam_unstable/navigation/EqVIOFilter.cpp

dellaert · 2026-03-28T17:25:32Z

gtsam/base/ProductLieGroup-inl.h

-  Jacobian1 D_g_second;
-  Jacobian2 D_h_first;
-  Jacobian2 D_h_second;
+  Jacobian1 D_g_first = Jacobian1::Zero(


This is regrettable- does this only fail for certain compilers/versions?

Yes, I noticed that it failed for the ubuntu-latest build, as a result of the template predictwithJacobian(...) in the EqVIOFilter. After backtracing a little bit the warning appears in compose/Logmap, which gets instantiated by the nested dynamic VioGroup during propagateState (VioGroup::Logmap(...)).

From my current understanding it is a false warning and zero-initializing fixes it. GCC can’t see that the Jacobians get assigned before they’re copied in this template path.

gtsam_unstable/navigation/EqVIOFilter.h

dellaert · 2026-03-28T17:40:34Z

gtsam/navigation/EquivariantFilter.h

+   */
+
+  /// Reset reference, covariance, and group estimate; sync manifold state.
+  void resetReferenceAndGroup(const M& xi_ref, const CovarianceM& P,


If we can avoid, would be better. And would we ever create a new xi_ref and a non-identity X? If we re-reference, would X not be identity?

Some updates (landmark add/remove / dimension changes) change xi_ref for bookkeeping/structure, not to the full current estimate. In that case, X stays non-identity to preserve the same physical estimate. I think if we forced identity the estimate would jump.

rohan-bansal · 2026-03-28T20:13:25Z

I removed the Snapshot struct tried to use the base EquivariantFilter as the single source of truth as much as possible.

Specifics:

Removed the snapshot struct (the duplicated state/cov/group copy)
Updated everything to read directly from:
- referenceState()
- groupEstimate()
- errorCovariance()
- stateEstimate()
For dynamic landmark add/remove, I rebuild(xi_ref, Sigma, X) and apply them atomically with resetReferenceAndGroup(...). I think this removes the necessity of maintaining a mirrored container struct.
Was also able to prune some of my additions to EquivariantFilter.h as a result.

Let me know what you think about these changes @dellaert.

dellaert

It seems to me that there’s still a lot done in the derived class that should be done in the base class. One of the goals here is to demonstrate how the templates can be used with a minimum of additional code.

It’s good that the shadow state is gone!

gtsam_unstable/navigation/EqVIOFilter.h

dellaert · 2026-03-28T23:01:41Z

gtsam_unstable/navigation/EqVIOFilter.h

+  static Matrix defaultCovariance(size_t nLandmarks);
+
+  /// Propagate covariance through linearized error dynamics for one effective IMU segment.
+  void propagateCovariance(const IMUInput& imu, double dt);


Why can EqF not do this for us?

The paper was vague about how covariance/state was propagated, so I defaulted to van Goor's code implementation, which propagated covariance once and then propagated the state piecewise over each IMU hold.

It is true that we can have the EqF do this for us. I changed the flow so that for each IMU hold we now call predict(lift, psi, etc.). This gets rid of a good chunk of code (which is nice), but also means the linearization is updated for each hold, which minorly changes the filter output; looks fine on the example, but will do broader tests later.

gtsam_unstable/navigation/EqVIOFilter.h

dellaert · 2026-03-28T23:02:36Z

gtsam_unstable/navigation/EqVIOFilter.h

+  void addNewLandmarks(const VisionMeasurement& measurement,
+                       const std::shared_ptr<const CameraModel>& camera);
+  /// Remove landmark by contiguous index in `cameraLandmarks`.
+  void removeLandmarkByIndex(int idx);


Do we need/use this?

Yes, these are used by the update step of the filter, which along with the internal filter update, also does some landmark bookkeeping/management (removes stale landmarks, rejects outliers, inserts new landmarks as they become available)

gtsam_unstable/navigation/EqVIOFilter.h

rohan-bansal added 20 commits March 22, 2026 14:40

transplant on top of develop

e36bf02

compat

3b2a492

prune unused

4c53f0b

reduce tests and combine files

010149a

move csv helpers into example file

2cda76c

fix uninitialized jacobian error in CI

45d1de2

fix propagation and simplify

683cef0

tighten impl, change ambiguous naming

c862d7b

zero initialize jacobian temporaries to fix CI again

d4e0710

add csv, update example, change headers, remove continuous lift velocity

0827b30

pruned old median depth function, moved single-use stuff to tests

f4d18a7

[skip ci] clean up filter a bit

f406bf2

[skip ci] misc.

8a92939

[skip ci] cherry-pick borglab#2473

1f5f012

rename view to snapshot, clean up example

4889ba6

[skip ci] generated some docs

b606d53

[skip ci] prune comments, explain things that should be explained better

70f7f14

[skip ci] better commenting pt 2

e268c83

[skip ci] simplify

58fc168

start ci

4a8d874

rohan-bansal marked this pull request as ready for review March 25, 2026 00:52

rohan-bansal requested a review from dellaert March 25, 2026 00:57

dellaert requested a review from Copilot March 28, 2026 17:23

Copilot started reviewing on behalf of dellaert March 28, 2026 17:23 View session

Copilot AI reviewed Mar 28, 2026

View reviewed changes

dellaert reviewed Mar 28, 2026

View reviewed changes

rohan-bansal added 2 commits March 28, 2026 11:34

factor out snapshot, remove double-owned vars

a37602f

xi_ref renames

36cccb5

rohan-bansal added 3 commits March 28, 2026 11:54

address copilot review

9c86457

cleanup

3332231

[skip ci] check diffs

eab0cc4

trigger ci

0523ea8

dellaert reviewed Mar 28, 2026

View reviewed changes

rohan-bansal added 8 commits March 28, 2026 18:56

address comments

5325b58

switch to keys from ids

3808432

new propagation

9b5e83b

prune

e643fc0

prune some more

5a2f94a

[skip ci] reorganize

4ec15dc

rename to predict/update

6ea2eb5

trigger ci

7d9496f

Conversation

rohan-bansal commented Mar 22, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

rohan-bansal commented Mar 25, 2026

Uh oh!

dellaert commented Mar 26, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

dellaert Mar 28, 2026

Choose a reason for hiding this comment

Uh oh!

rohan-bansal Mar 28, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

dellaert Mar 28, 2026

Choose a reason for hiding this comment

Uh oh!

rohan-bansal Mar 28, 2026

Choose a reason for hiding this comment

Uh oh!

rohan-bansal commented Mar 28, 2026

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!

dellaert Mar 28, 2026

Choose a reason for hiding this comment

Uh oh!

rohan-bansal Mar 29, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

dellaert Mar 28, 2026

Choose a reason for hiding this comment

Uh oh!

rohan-bansal Mar 29, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

rohan-bansal commented Mar 22, 2026 •

edited

Loading

rohan-bansal Mar 29, 2026 •

edited

Loading