feat(tools): drive R-IEKF through the real EuRoC pipeline - #429
Conversation
Adds the end-to-end harness that runs the right-invariant (R-IEKF) VIO backend (#347/#348) through the validated VioEstimator front-end / track manager on real EuRoC, so it can be compared head-to-head with the standard MSCKF instead of the confounded hand-rolled Monte-Carlo loop. - invariant_backend_adapter.hpp: a VioBackend-conforming wrapper that bridges InvariantVioBackend's minimal API (set_nav / process_imu / process_camera-normalized / nav) to the VioEstimator contract. Boot- straps from a static, gravity-aligned IMU window; unprojects pixels to bearings; maps SE2(3) back to NavState. Uses the Joseph FullCovariance form (the backend default) — the square-root form diverges on real data. - vio_pipeline: a --invariant path (run_euroc_invariant) mirroring the standard EuRoC run, reporting ATE + mean position sigma so the two backends are measured on the same sequence with the same front end. - msckf_invariant_backend: guard the update against a non-finite dx — snapshot the covariance and reject the track (it never happened) rather than abort in SO3::normalize on a degenerate real-data measurement. Result on V1_01 / V2_03: the R-IEKF now runs stably end to end, but underperforms the standard MSCKF on both ATE (1.3 m vs 0.27 m) and position-sigma consistency. The position-norm ATE/sigma ratio cannot isolate the yaw-gauge effect the R-IEKF targets (standard MSCKF is already ~calibrated at 2.1 on position); a gauge-anchored attitude-NEES metric is the needed next step (the #365 acceptance criterion) and is a follow-up. Relates to #365 Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
|
No actionable comments were generated in the recent review. 🎉 ℹ️ Recent review info⚙️ Run configurationConfiguration used: Path: .coderabbit.yaml Review profile: ASSERTIVE Plan: Pro Run ID: 📒 Files selected for processing (1)
WalkthroughAdds an invariant backend adapter, a new invariant EuRoC execution path, mean position sigma reporting, adapter tests, and a non-finite correction guard in the invariant MSCKF backend. ChangesInvariant backend adapter and EuRoC pipeline
Estimated code review effort🎯 4 (Complex) | ⏱️ ~60 minutes Possibly related issues
Possibly related PRs
🚥 Pre-merge checks | ✅ 4 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (4 passed)
✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
Comment |
|
@coderabbitai review |
✅ Action performedReview finished.
|
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 7
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp`:
- Around line 248-255: The covariance update path in the MSCKF invariant backend
only validates dx after cov_.update(), so a non-finite covariance/factor can
still be committed even when the correction looks fine. In the update logic in
the covariance update routine, restore cov_snapshot not only when dx is bad but
also when the updated cov_/S becomes non-finite, and only return success after
both the correction and the updated covariance state are verified; use the
existing symbols cov_, cov_snapshot, and cov_.update() to locate the fix.
- Around line 250-255: The finite check in the `msckf_invariant_backend` loop is
too restrictive because it forces each `T` value through a `double` conversion,
which breaks the template’s duck-typed `math::Scalar` contract. Update the `dx`
validation in the backend to use a `T`-aware finiteness check or customization
point instead of `std::isfinite(static_cast<double>(v))`, or else explicitly
constrain the backend’s `T`/`Scalar` requirements to double-convertible types if
that is the intended scope.
In `@tests/tools/invariant_backend_adapter.cpp`:
- Around line 94-100: The timestamp assertion in the camera-path test is too
weak because it only checks that the state timestamp is positive, which is
already guaranteed by bootstrap in invariant_backend_adapter::process_imu/init
state handling. Update the test around a.process_camera(...) to capture
a.current_state().timestamp_s before the call and then REQUIRE that it changes
or advances afterward, so the check specifically validates that process_camera()
updates the timestamp.
- Around line 59-62: The test in invariant_backend_adapter should verify the
pre-bootstrap boundary, not only the final initialized state. Update the
scenario around a.process_imu and initialized() to assert that the adapter
remains uninitialized before the sample window is full, then becomes initialized
only after enough stationary samples have been processed, using the existing
invariant_backend_adapter behavior to pin the threshold.
In `@tools/src/vio_pipeline.cpp`:
- Around line 557-559: The EuRoC evaluation paths currently treat an empty pose
association as a zero ATE and still return success, which masks missing ground
truth, timestamp mismatches, or an untracked trajectory. In both `run_euroc()`
and `run_euroc_invariant()`, add a guard after `ev::associate<T>(traj, gt,
0.02)` so that if `assoc.estimated` is empty the run fails instead of setting
`out.ate_rms_m` to 0 and returning true; keep the normal RMSE computation only
when association succeeds.
- Around line 152-154: The pos_sigma helper is masking invalid covariance by
clamping a non-positive trace to zero, which turns NaN into a false
zero-confidence result. Update pos_sigma to preserve NaN/invalid values instead
of forcing 0.0, and make the ATE reporting path in the harness that formats the
sigma value (the code around the ATE/sigma output) propagate or explicitly flag
invalid covariance rather than printing 0.0.
- Around line 589-604: The invariant path in vio_pipeline.cpp is hardcoding the
standard EuRoC intrinsics/extrinsics, which duplicates the same calibration data
used elsewhere and risks divergence between comparison modes. Extract the shared
EuRoC camera parameters into a single reusable constant/helper and have the
Estimator setup reuse it instead of constructing the Camera, R_imu_cam, and
p_imu_cam inline in this block.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Path: .coderabbit.yaml
Review profile: ASSERTIVE
Plan: Pro
Run ID: 9d590cff-29db-4a81-b0be-c7a0cbadf803
📒 Files selected for processing (4)
sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpptests/tools/invariant_backend_adapter.cpptools/include/branes/tools/invariant_backend_adapter.hpptools/src/vio_pipeline.cpp
| const Cov cov_snapshot = cov_; | ||
| const std::vector<T> dx = cov_.update(H, std::span<const T>{proj.r}, std::span<const T>{R_diag}); | ||
| for (const T v : dx) { | ||
| if (!std::isfinite(static_cast<double>(v))) { | ||
| cov_ = cov_snapshot; | ||
| return false; | ||
| } | ||
| } |
There was a problem hiding this comment.
🩺 Stability & Availability | 🟡 Minor | ⚡ Quick win
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# Inspect Cov::update to see whether cov_ can emit a finite dx with a non-finite covariance.
fd -e hpp -e h 'covariance' sdk/include | xargs -r rg -nP -C4 '\bupdate\s*\('
rg -nP -C3 'pos_sigma' tools/src/vio_pipeline.cppRepository: branes-ai/cortex
Length of output: 6679
🏁 Script executed:
#!/bin/bash
set -euo pipefail
# Show the invariant backend guard in context.
sed -n '232,266p' sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp
# Show the full-covariance update implementation.
sed -n '106,170p' sdk/include/branes/sdk/msckf/covariance.hpp
# Show the square-root covariance update implementation and its guard.
sed -n '83,135p' sdk/include/branes/sdk/msckf/sqrt_covariance.hpp
# Show the telemetry path that consumes covariance.
sed -n '620,705p' tools/src/vio_pipeline.cppRepository: branes-ai/cortex
Length of output: 10830
🏁 Script executed:
#!/bin/bash
set -euo pipefail
# Locate the square-root helpers used by the update path.
rg -n "retriangularize|sqrt_" sdk/include/branes/sdk -g '*.hpp'
# Read the helper implementation and any numeric guards around it.
fd -e hpp sdk/include/branes/sdk | xargs -r rg -n -C4 'retriangularize|sqrt_'Repository: branes-ai/cortex
Length of output: 3755
🏁 Script executed:
#!/bin/bash
set -euo pipefail
# Inspect the square-root covariance API and QR helper.
sed -n '1,220p' sdk/include/branes/sdk/msckf/sqrt_covariance.hpp
sed -n '1,180p' sdk/include/branes/sdk/msckf/qr.hpp
# Inspect the invariant backend type aliases and covariance access path.
sed -n '1,320p' sdk/include/branes/sdk/msckf/msckf_invariant_backend.hppRepository: branes-ai/cortex
Length of output: 25506
Restore the covariance snapshot on any bad update, not just a bad correction. sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp:248-255
cov_.update() mutates the stored covariance before dx is checked. If the update leaves cov_/S non-finite while dx happens to stay finite, this path still commits the poisoned factor and covariance()/pos_sigma() will read it. Check the updated covariance (or its factor) before returning success, or narrow the comment to say only the correction is screened.
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp` around lines 248 -
255, The covariance update path in the MSCKF invariant backend only validates dx
after cov_.update(), so a non-finite covariance/factor can still be committed
even when the correction looks fine. In the update logic in the covariance
update routine, restore cov_snapshot not only when dx is bad but also when the
updated cov_/S becomes non-finite, and only return success after both the
correction and the updated covariance state are verified; use the existing
symbols cov_, cov_snapshot, and cov_.update() to locate the fix.
- msckf_invariant_backend: use an unqualified isfinite (ADL) for the δx
guard instead of narrowing each component to double, so a custom
math::Scalar (Universal posit) supplies its own — honors the
no-hardcoded-double rule. δx is documented as the sentinel: a poisoned
covariance resurfaces as a non-finite δx on the next update.
- vio_pipeline: extract the EuRoC cam0 calibration into a shared
euroc_cam0() helper so the MSCKF and R-IEKF runs are driven from
byte-identical intrinsics/extrinsics (a fair comparison can't drift).
- vio_pipeline: fail the EuRoC run when pose association is empty rather
than reporting a perfect ATE 0 — an empty association means missing GT,
timestamp drift, or an untracked trajectory.
- vio_pipeline: stop masking a non-finite position σ as 0 (in pos_sigma
and the over-confidence print) so a poisoned covariance is surfaced,
which is exactly what this harness exists to expose.
- tests: assert the adapter stays uninitialized before the bootstrap
window fills, and that process_camera advances the state timestamp
(compare before vs after) instead of only checking it is positive.
V1_01 R-IEKF ATE is unchanged at 1.31 m after the calibration refactor.
Relates to #365
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
|
Thanks — addressed in 6914e50. Per comment:
|
|
@coderabbitai review |
✅ Action performedReview finished.
|
There was a problem hiding this comment.
♻️ Duplicate comments (1)
tests/tools/invariant_backend_adapter.cpp (1)
59-67: 🎯 Functional Correctness | 🔵 Trivial | ⚡ Quick winBoundary check is too loose to catch the regression the original comment targeted.
This partially addresses the earlier feedback but checks
initialized()at sample 10, far belowkInitSamples(150, perinvariant_backend_adapter.hpp). A regression that drops the threshold to, say, 100 samples would still pass bothREQUIRE_FALSE(at 10) and the laterREQUIRE(initialized())(at 200). Move the negative assertion to right before the window fills (kInitSamples - 1) to actually pin the off-by-one boundary, as originally requested.♻️ Suggested tightening
- for (int i = 0; i < 10; ++i) - a.process_imu(stationary_sample(0.005 * i)); - REQUIRE_FALSE(a.initialized()); - - // Once a full static window accumulates, init fires. - for (int i = 10; i < 200; ++i) + for (int i = 0; i < 149; ++i) + a.process_imu(stationary_sample(0.005 * i)); + REQUIRE_FALSE(a.initialized()); + + // Once a full static window accumulates, init fires. + for (int i = 149; i < 200; ++i)🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@tests/tools/invariant_backend_adapter.cpp` around lines 59 - 67, The boundary assertion in the IMU bootstrap test is too early to catch a lowered initialization threshold; update the negative check in the test around process_imu and initialized() to assert false at the last pre-init sample just before the bootstrap window fills, using the kInitSamples boundary from invariant_backend_adapter.hpp. Keep the existing positive assertion after the full window accumulates, but move the failing case to kInitSamples - 1 so the test pins the off-by-one behavior and detects regressions that initialize too soon.
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Duplicate comments:
In `@tests/tools/invariant_backend_adapter.cpp`:
- Around line 59-67: The boundary assertion in the IMU bootstrap test is too
early to catch a lowered initialization threshold; update the negative check in
the test around process_imu and initialized() to assert false at the last
pre-init sample just before the bootstrap window fills, using the kInitSamples
boundary from invariant_backend_adapter.hpp. Keep the existing positive
assertion after the full window accumulates, but move the failing case to
kInitSamples - 1 so the test pins the off-by-one behavior and detects
regressions that initialize too soon.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Path: .coderabbit.yaml
Review profile: ASSERTIVE
Plan: Pro
Run ID: b3b28be8-a019-46e0-a1fe-405181fd467e
📒 Files selected for processing (3)
sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpptests/tools/invariant_backend_adapter.cpptools/src/vio_pipeline.cpp
Assert the adapter is still uninitialized one sample short of the window and initializes exactly as it fills, instead of checking far below the threshold — the off-by-one pin also catches a silently-lowered threshold (CodeRabbit follow-up). Relates to #365 Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
|
Tightened in c14f025 — the negative assertion now sits at |
|
@coderabbitai review |
✅ Action performedReview finished.
|
1 participant
Summary
Adds the end-to-end harness that runs the right-invariant (R-IEKF) VIO backend (#347/#348) through the validated
VioEstimatorfront-end / track manager on real EuRoC — the path #365 calls for, replacing the confounded hand-rolled Monte-Carlo loop. With the harness in place, R-IEKF and the standard MSCKF can be measured on the same sequence with the same front end.This lands the infrastructure and an honest, inconclusive result — not a win. See "Result" below.
Changes
tools/include/branes/tools/invariant_backend_adapter.hpp(new) — aVioBackend-conforming wrapper bridgingInvariantVioBackend's minimal API (set_nav/process_imu/process_camera-normalized /nav) to theVioEstimatorcontract. Bootstraps from a static, gravity-aligned IMU window; unprojects pixels → bearings; maps SE₂(3) →NavState. Uses the JosephFullCovarianceform (the backend default); the square-root form diverges on real data.tools/src/vio_pipeline.cpp— a--invariantpath (run_euroc_invariant) mirroring the standard EuRoC run, reporting ATE + mean position σ.sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp(shipped SDK) — guard the update against a non-finite δx: snapshot the covariance and reject the track (as if it never happened) rather than abort inSO3::normalizeon a degenerate real-data measurement. Strictly a crash→rejection conversion; healthy updates are unaffected.tests/tools/invariant_backend_adapter.cpp(new) — gate: static-window bootstrap (levels gravity, zero velocity) + VioEstimator contract.Result — R-IEKF vs standard MSCKF (same binary, real EuRoC)
The R-IEKF now runs stably end to end (the
FullCovarianceswitch fixed a 10⁴⁰-m divergence), but underperforms the standard MSCKF on both ATE and position-σ consistency. Critically, the position-norm ATE/σ ratio cannot isolate the yaw-gauge effect the R-IEKF targets — standard MSCKF is already ~calibrated at 2.1 on the position norm, because the leak is rotational. The Jacobian-level evidence (yaw leak 0.39 → 2e-16, merged in #426) remains the real proof of the fix.Follow-up (the #365 acceptance criterion): a gauge-anchored attitude-NEES metric (yaw vs roll/pitch split) so the comparison measures rotational consistency where R-IEKF should win, plus seed/tuning parity. Tracked as the next step on #365.
Test results
Test plan
Relates to #365
🤖 Generated with Claude Code
Summary by CodeRabbit