EqVIO Filter Implementation

[rohan-bansal]

Changelog:

1. 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.
2. 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.
3. 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.
4. 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 around 7MB in size.

Working example output below!

CSV replay complete.
Events: 6602, IMU: 6001, vision frames: 601, vision features: 13316
Measurement noise variance (normalized): 1.77557e-05
Filter time: 1403715303.2621431
Landmarks: 32
Pose translation:   1.178667103   2.537093355 0.07318948356
GT pose translation:    1.1576   2.51599 0.0838289
Velocity: 0.0003766258116   -0.2772464545   0.05656939892
GT velocity: 0.0130831 -0.269437 0.0995449

[rohan-bansal]

Ready for review @dellaert :)

[dellaert]

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.

[Copilot]

Pull request overview

Copilot reviewed 20 out of 21 changed files in this pull request and generated 12 comments.

[dellaert]

@rohan-bansal ball in your camp

[rohan-bansal]

@dellaert I addressed the comments. I think reverting to Mahalanobis probabalistic gating is not necessary, the simplified version works well after fixing the xi0 initialization bug and Cheirality errors.

I also verified the filter performance on the EqVIO EuRoC MAV dataset, and replaced the existing example data with a longer version that better shows how the filter performs :) Updated the top-level PR comment to display what the output looks like.

[dellaert]

Some more comments and I’m also rereading eqvio paper because I want to know the relationship with current inverse depth impl in GTSAM

[rohan-bansal]

@dellaert I took a look at the InverseDepth implementations existing in GTSAM, which seem to mostly be factor-graph machinery. It represents landmarks as inverse-depth variables and computes reprojection-factor errors. In EqVIO, landmarks are still stored as Point3 but the error coordinates inverse-depth-like.

Replacing the EqVIO chart with GTSAM’s inverse-depth representation would mean that we need to factor out specific inverse-depth logic into geometric helpers, and also do a bigger refactor of state layout, covariance blocks, landmark initialization, and all the EqF Jacobians.

Besides this, I addressed your comments and tightened the codebase in some other areas after doing another sweep.

Also, I am going to context-switch and work on AwesomeEqF in parallel from this current code snapshot.

[dellaert]

Cool! LGTM!