fix(sdk): set EuRoC extrinsics + gravity-align init; surface VIO divergence - #209
Merged
Conversation
WalkthroughAdds gravity-direction-only IMU bootstrap and init telemetry; rewrites MSCKF init flow to prefer gravity-align after timeout; adds tests for gravity-align; instruments vio_bench with divergence detection, EuRoC extrinsics, --fail-fast, and serializes estimator health in BenchReport. ChangesEstimator Initialization Diagnostics & Health Tracking
Estimated code review effort🎯 4 (Complex) | ⏱️ ~45 minutes Possibly related PRs
🚥 Pre-merge checks | ✅ 4 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (4 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 4
🤖 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 `@bench/include/branes/bench/report.hpp`:
- Line 197: The health-table label string written to the output stream contains
raw pipe characters causing Markdown table mis-parsing; update the stream
insertion that emits the label (the line using os << "| Max |position| (m) | "
<< r.max_position_m << " |\n\n"; referencing r.max_position_m) to escape the
pipe characters (e.g., replace '|' with '\|' in the literal) so the Markdown
renderer treats them as literal characters rather than column separators.
In `@bench/src/vio_bench.cpp`:
- Around line 357-366: Don't hard-code r.extrinsics_identity = false; instead
query the estimator's stored calibration/extrinsics (e.g. via
est.backend().calibration() or whatever holds the cam0 extrinsics exposed by
est.backend(), or init.extrinsics if present) and set r.extrinsics_identity by
comparing that extrinsics to an identity transform within a small tolerance:
check that rotation is close to identity (e.g. quaternion ~ [1,0,0,0] or
rotation matrix ~ I) and translation is near zero, and assign true only when
both checks pass; use the same numeric tolerance used elsewhere in the codebase
for pose comparisons.
- Around line 281-302: The code currently pushes est.current_pose() into traj
(traj.push_back) and updates peak_speed/max_pos before performing the divergence
guard, which can record NaN/Inf or extreme values; reorder the logic so the
divergence check using ns, speed, dist and the divergence handling (setting
diverged, divergence_frame, divergence_time_s, printing, and fail_fast behavior)
runs before any mutation of traj, peak_speed, or max_pos, and only when the
state passes that check do you append to traj and update peak_speed/max_pos (or
skip recording entirely if diverged); adjust uses of frame_idx and fail_fast
exactly as currently implemented so behavior on early exit remains unchanged.
In `@sdk/include/branes/sdk/msckf_backend.hpp`:
- Around line 275-300: The code erases one sample on every failed static attempt
(init_gyro_/init_accel_.erase(...)) so the buffer never reaches kInitMaxSamples
and the fallback at line 286 (gravity-align/identity bootstrap via
initializer_.try_gravity_align and apply_init/InitMethod) is never reached; stop
removing a sample on each failed check and instead only trim the buffer when you
append a new sample and its size exceeds kInitMaxSamples (or remove the erase
branch entirely so the window can grow to kInitMaxSamples), ensuring init_gyro_
and init_accel_ can reach kInitMaxSamples and trigger
try_gravity_align/apply_init.
🪄 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: 08ca0e5c-0288-45c7-a61e-4f34f73fb9c1
📒 Files selected for processing (5)
bench/include/branes/bench/report.hppbench/src/vio_bench.cppsdk/include/branes/sdk/imu_init.hppsdk/include/branes/sdk/msckf_backend.hpptests/sdk_imu_init_test.cpp
…rgence vio_bench diverged catastrophically on V1_01_easy (ATE 26,574 m vs a 0.5 m gate). Two faults, plus no way to see either without a full 13-minute run: 1. vio_bench never set the camera↔IMU extrinsics, so every camera update was geometrically inconsistent with the IMU-propagated pose (EuRoC cam0 is rotated ~90° + offset ~7 cm from the body). This was the dominant cause here. Set the published cam0 T_BS (matrix→quaternion). 2. MsckfBackend's init fallback used a blind identity attitude when the static window never settled. On a tilted/airborne start that leaves gravity uncancelled in propagation → quadratic position blowup. Replace it with ImuInitializer::try_gravity_align: roll/pitch from the mean specific force (yaw/bias stay zero). A robustness fix for moving starts; full visual-inertial (try_dynamic) needs an SfM bootstrap — see the documented seam. Diagnostics so a bad run is identified immediately, not inferred from ATE: - MsckfBackend::init_diagnostics() — method (static/gravity_align/identity), platform tilt, gravity residual, biases. - vio_bench per-frame divergence guard + a top-line verdict (OK / above-gate / DIVERGED at frame N), an "Estimator health" report section, --fail-fast. Result on V1_01_easy: ATE 26,574 m → 0.305 m (within gate); the health block correctly reports init method `static`, which is what pinned the real cause on the extrinsics rather than the init fallback. Tests: new try_gravity_align coverage (moving-start recovery + free-fall rejection); full suite green on clang and gcc (151 tests). Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
…nsics flag CodeRabbit review on #209: - msckf_backend: the init buffer was trimmed on every failed static attempt, pinning it at kInitSamples so it never reached kInitMaxSamples — the gravity-align/identity fallback was unreachable (the bug predates this PR; it made the new gravity-align path dead code). Slide a fixed kInitSamples window for the static check and count total samples seen for the timeout, so the fallback fires. New backend test: a never-stationary tilted start now initializes via GravityAlign within a few degrees. - vio_bench: derive extrinsics_identity from the built calibration (rotation angle ≈ 0 and translation ≈ 0) instead of asserting a constant, so it stays truthful if the extrinsics are ever dropped. - report: reword the "Max position norm (m)" health label — the old label contained raw '|' characters that broke the Markdown table. Not changed: the divergence-guard ordering. peak_speed/max_pos are meant to capture the divergence spike, so they must update on the offending frame; std::max(running, NaN) keeps the running value and the verdict supersedes a NaN ATE — reordering to skip recording would hide the magnitude we report. Tests green on clang and gcc. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
Ravenwater
force-pushed
the
fix/vio-init-divergence-diagnostics
branch
from
May 29, 2026 19:01
401cab0 to
f34ce0c
Compare
Contributor
Author
|
Thanks @coderabbitai — addressed 3 of 4, and rebased onto
Not changed — divergence-guard ordering ( |
|
Tip For best results, initiate chat on the files or code changes.
Fixes acknowledged:
Divergence-guard ordering — accepted. 🐇✓ ✏️ Learnings added
|
There was a problem hiding this comment.
Actionable comments posted: 3
🤖 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 `@bench/include/branes/bench/report.hpp`:
- Around line 201-203: The message printed when r.init_method == "gravity_align"
incorrectly states biases start at zero; update the output string in the
gravity-align branch (the conditional checking r.init_method == "gravity_align")
to explicitly state that roll/pitch are set from mean specific force and yaw is
initialized to zero, and mention that biases may be non-zero (or optionally
seeded) depending on motion—i.e., separate the definitive "yaw starts at zero"
claim from any bias behavior and remove the absolute "biases start at zero"
wording so the note accurately reflects conditional bias initialization.
- Around line 184-187: The report currently treats r.accuracy_within_gate==false
the same whether accuracy is truly above the gate or accuracy is unavailable,
causing misleading "ATE 0 m above gate" output; update the reporting logic for
the Report struct (symbols: r.accuracy_within_gate, r.ate_m, r.ate_gate_m) by
adding and checking a new flag (e.g., r.accuracy_available or
r.has_ground_truth) or a dedicated verdict path for unavailable accuracy metrics
and print a distinct message like "accuracy unavailable / ground truth missing"
when that flag is false, otherwise keep the existing OK vs above-gate branches.
Ensure the new flag is set by the code that computes ATE (e.g., vio_bench.cpp)
so unavailable ground-truth does not default to ate_m==0 and a false
within_gate.
In `@bench/src/vio_bench.cpp`:
- Around line 301-308: When fail_fast triggers inside the divergence check (the
block that sets diverged, divergence_frame, divergence_time_s and optionally
breaks when fail_fast is true), record the timestamp of the last processed frame
(e.g., last_included_time_s or last_frame_t_s) before breaking and then compute
sequence_duration_s from images.front().t_s to that recorded timestamp instead
of images.back().t_s; ensure any reported real_time_factor and other
duration-derived metrics use that last-included timestamp so partial runs report
duration consistent with the frames actually processed. Also update any code
that currently derives sequence_duration_s from images.back() to prefer the
recorded last_included_time_s when diverged && fail_fast was used.
🪄 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: 41a0b2cd-0d84-4c56-b685-9eddd594546e
📒 Files selected for processing (6)
bench/include/branes/bench/report.hppbench/src/vio_bench.cppsdk/include/branes/sdk/imu_init.hppsdk/include/branes/sdk/msckf_backend.hpptests/sdk_imu_init_test.cpptests/sdk_msckf_backend_test.cpp
…rtial-run duration CodeRabbit incremental review on #209: - Distinguish "accuracy unavailable" from "ATE above gate". With no usable ground truth, ate_m stayed 0 and the verdict printed "ATE 0 m above gate", hiding the real failure. Add BenchReport::accuracy_available (set from the ground-truth match count) and a dedicated verdict branch. - --fail-fast reported the full-sequence duration for a partial run, inflating real_time_factor. Derive sequence_duration_s from the span of frames actually processed (traj first→last) instead of images.front()/back(). - Reword the gravity-align health note: roll/pitch from mean specific force and yaw start at zero, but the gyro bias is seeded only when the window is rotation-quiet (else zero) — the old note wrongly said biases are always zero. Builds + bench tests green on clang and gcc. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
Contributor
Author
|
Round 2 addressed (incremental review):
Builds + bench tests green on clang and gcc. |
There was a problem hiding this comment.
Actionable comments posted: 1
🤖 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 `@bench/include/branes/bench/report.hpp`:
- Around line 186-188: The report currently prints zeroed ATE/RPE and marks
Energy as "ATE above gate" even when r.accuracy_available is false; update all
places that render the Accuracy table and the Energy verdict (references: the
variables/fields r.accuracy_available, r.ate, r.rpe and the functions/sections
that write the Accuracy table and Energy summary in report.hpp) to check
r.accuracy_available first and, if false, emit a clear "unavailable" / "n/a"
marker (and skip showing or evaluating numeric ATE/RPE or gate-comparison logic)
so the top-line verdict, Accuracy table, and Energy section consistently
indicate accuracy is unavailable rather than showing zeros or failed-gate text;
apply the same conditional logic where ATE/RPE are printed or used (the other
similar blocks flagged in the comment).
🪄 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: 5f484544-a4c1-497d-9c59-8a7fdeaf1e9e
📒 Files selected for processing (2)
bench/include/branes/bench/report.hppbench/src/vio_bench.cpp
…eport CodeRabbit follow-up on #209: the top-line verdict handled missing ground truth, but the Accuracy table still printed zeroed ATE/RPE and the Energy section still read "n/a (ATE above gate)" — contradicting the verdict. Gate both on accuracy_available: the table shows "n/a (no usable ground truth)" and the Intelligence/Watt line shows "n/a (accuracy unavailable)". (JSON keeps the numeric fields, which already carry an explicit `available` flag.) Bench tests green on clang and gcc. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
Contributor
Author
|
Round 3: applied |
There was a problem hiding this comment.
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
bench/include/branes/bench/report.hpp (1)
150-151:⚠️ Potential issue | 🟡 Minor | ⚡ Quick winNon-finite floats produce invalid JSON when the estimator diverges.
json_kvstreams doubles directly, so NaN/Inf render asnan/inf— tokens that are not valid JSON. PR objectives confirm these values can occur when divergence happens. Downstream JSON parsers will choke even thoughdiverged: trueis present.Consider sanitizing non-finite values (emit
nullor a sentinel like1e308) injson_kv, or add a dedicated overload for fields that can blow up:Proposed helper for safe double output
inline void json_kv(std::ostream& os, const char* k, double v, bool comma = true) { - os << " \"" << k << "\": " << v << (comma ? ",\n" : "\n"); + os << " \"" << k << "\": "; + if (std::isfinite(v)) + os << v; + else + os << "null"; // or a finite sentinel; null keeps JSON valid + os << (comma ? ",\n" : "\n"); }(Requires
#include <cmath>forstd::isfinite.)🤖 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 `@bench/include/branes/bench/report.hpp` around lines 150 - 151, The JSON writer is emitting non-finite doubles (NaN/Inf) for fields like peak_speed_mps and max_position_m which produces invalid JSON; update detail::json_kv to sanitize double values by checking std::isfinite (include <cmath>) and, when non-finite, emit JSON null (or a chosen sentinel) instead of streaming the raw value, or add a dedicated overload detail::json_kv(os, key, double val, ...) that performs this finite-check and uses it for fields that can diverge (e.g., calls from the report serialization for r.peak_speed_mps and r.max_position_m).
🤖 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.
Outside diff comments:
In `@bench/include/branes/bench/report.hpp`:
- Around line 150-151: The JSON writer is emitting non-finite doubles (NaN/Inf)
for fields like peak_speed_mps and max_position_m which produces invalid JSON;
update detail::json_kv to sanitize double values by checking std::isfinite
(include <cmath>) and, when non-finite, emit JSON null (or a chosen sentinel)
instead of streaming the raw value, or add a dedicated overload
detail::json_kv(os, key, double val, ...) that performs this finite-check and
uses it for fields that can diverge (e.g., calls from the report serialization
for r.peak_speed_mps and r.max_position_m).
ℹ️ Review info
⚙️ Run configuration
Configuration used: Path: .coderabbit.yaml
Review profile: ASSERTIVE
Plan: Pro
Run ID: 71eb6824-2a4a-4832-984e-5b7fc3c4858a
📒 Files selected for processing (1)
bench/include/branes/bench/report.hpp
This was referenced May 29, 2026
Merged
2 tasks
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Problem
vio_benchdiverged catastrophically on EuRoC V1_01_easy — ATE 26,574 m against a 0.5 m gate — and nothing surfaced the failure until a full ~13-minute run printed the giant number.Root cause (two faults)
vio_benchset intrinsics but leftcal.extrinsicsat identity. EuRoC cam0T_BSis a ~90° rotation + ~7 cm offset from the IMU body frame, so every camera update was geometrically inconsistent with the IMU-propagated pose and corrupted the filter.MsckfBackendseeded an identity attitude. On a tilted/airborne start that leaves gravity uncancelled in propagation (a_world = R·a + gravity) → quadratic position blowup. (Not the trigger for V1_01_easy specifically — static init does succeed there — but a real latent bug for genuinely moving starts.)Fix
vio_benchnow sets the published cam0T_BS(matrix→quaternion helper, sinceSO3has no matrix ctor).ImuInitializer::try_gravity_align— roll/pitch from the mean specific force when no static window exists (yaw/biases stay zero, gyro mean trusted as bias only when rotation-quiet). Replaces the blind identity fallback. Full visual-inertial init (try_dynamic) needs an SfM/two-view bootstrap that doesn't exist yet — left as a documented seam.Diagnostics (identify failure modes immediately)
MsckfBackend::init_diagnostics()— init method (static/gravity_align/identity), platform tilt, gravity residual, biases.vio_benchper-frame divergence guard + top-line verdict (✅ OK /--fail-fastflag.These paid off immediately: the health block reported init method
static, which is what pinned the real cause on the extrinsics rather than the init fallback I first suspected.Result (V1_01_easy)
static(tilt 112°, gravity residual 0.0018)Test
try_gravity_alignunit tests: moving-start recovery (<5° gravity error) and free-fall rejection.Note
Branched off
main; touchesbench/src/vio_bench.cpp, which also changes in #208 (progress output + docs). Expect a small merge conflict in the bench loop — whichever merges first, I'll rebase the other.🤖 Generated with Claude Code
Summary by CodeRabbit
New Features
Tests