Loosely-coupled VIO pipeline (Stage 0b + 0c): IMU preintegration BA residual, accel-bias estimation, rotation coupling, VI initialization#3
Open
rsasaki0109 wants to merge 13 commits into
Conversation
…rks_ races
Two TSan-flagged data race classes in the async pipeline:
1. Frame::landmarks_ vs onBACompleted
- Writers on tracking thread (assignFrameLandmarksFromInliers,
initializeWithDepth, mono init, trackReferenceKeyframe, relocalize,
reinitialize) now hold current_frame_->mutex_ around landmarks_
writes.
- onBACompleted path readers (countValidFrameLandmarks,
recomputeCurrentPose) snapshot under the same mutex_.
- Keyframe(Frame::Ptr) ctor uses frame->snapshotLandmarks() helper.
- Frame::mutex_ is now mutable so snapshotLandmarks() can be const.
2. Keyframe::landmarks_ vs LocalMapping::createNewMapPoints writes
- needNewKeyframe snapshots reference_keyframe_->landmarks_.
- relocalize candidate loop and best-candidate assign phase snapshot
cand.kf->landmarks_ / best_candidate.kf->landmarks_ under each
kf->mutex_ before iterating; avoids holding two container mutexes
at once (matches local_mapping.cc convention).
Validation (build_codex, local):
- ctest: 59/59 passed
- check_regression_gate xyz_mono_head_repro: 0.028136 m (same SHA)
- check_regression_gate room_mono_head_repro: 0.176506 m (same SHA)
- check_regression_gate xyz_depth_head_repro: 0.011042 m (same SHA)
- check_regression_gate room_depth_head_repro: 0.079914 m (same SHA)
- TSan room_mono 250f: Frame::landmarks_ race gone; Keyframe::landmarks_
races in relocalize / needNewKeyframe gone. Residual TSan warnings are
pre-existing orthogonal issues (Map container API, Pose SSE races,
LocalMapping control flags).
Adds a neutral IMU measurement record (svslam::ImuEntry, accel + gyro + timestamp) in src/sensors/imu.h, and extends EurocDataset to load mav0/imu0/data.csv when present. - Silently ignored when imu0 is absent (synthetic test_seq has none). - New accessors: hasImu(), allImu(), getImuBetween(t0, t1). - Parses the EuRoC 7-column CSV (ts_ns, wx..wz, ax..az) and sorts by ts. Tests: - tests/test_euroc_dataset.cc: LoadsImuDataWhenPresent, SilentlySkipsMissingImu (2 new tests, 61/61 total passing). This is scaffolding for VIO Stage 0b.a (preintegrator), 0b.b (velocity state), and 0b.c (accel -> velocity model); no runtime behavior change yet.
…e 0b.b) Adds velocity_ (world-frame, m/s), accel_bias_, gyro_bias_ and has_velocity_ to Frame and Keyframe. Keyframe ctor copies the current Frame state at construction time. Purely scaffolding: no writer or BA residual touches these fields yet. Prepares Frame::Ptr / Keyframe::Ptr as the carrier for IMU preintegration results (0b.a) and for a future velocity-prior residual. Tests: 61/61 passing (no behavior change).
Minimal header-only IMU preintegration class: - integrate(accel, gyro, dt) accumulates delta_R, delta_v, delta_p over an interval using Euler integration in the start-of-interval frame. - predict(R_i, v_i, p_i, g) -> (R_j, v_j, p_j) propagates a keyframe state across the accumulated interval, applying the world-frame gravity vector. - reset(accel_bias, gyro_bias) re-anchors biases for the next interval. Intentionally omits bias Jacobians and noise covariance — those belong with a real VIO residual in BA (future work). This header-only skeleton is callable from Tracking for state prediction and can be upgraded later without touching call sites beyond the constructor. Tests: 5 new in tests/test_imu_preintegrator.cc covering zero motion, constant accel delta_v/delta_p, predict including gravity, reset, and bias subtraction. 66/66 total passing.
…ge 0b.c) Adds std::vector<ImuEntry> Tracking::imu_buffer_ and populates it from EurocDataset::allImu() when --accel is requested on an EuRoC sequence. The IMU accel channel is also mirrored into the existing accel_buffer_ so gravity alignment / stationary detection paths work unchanged. Net effect on the runtime: - EuRoC + --accel now enables gravity prior via IMU accel (previously the --accel flag was silently disabled on EuRoC because it only consulted TUM accelerometer.txt). - Full IMU (accel + gyro) is retained on tracker->imu_buffer_ for a future VIO path that plugs ImuPreintegrator (0b.a) into the motion model (0b.c future work). Validation: - ctest: 66/66 passing, same SHAs for xyz_mono_head_repro (0.028136) and room_mono_head_repro (0.176506), no regression.
… 0b.e / 0b.f / 0c.b)
Promotes the 0b.{a,b,c,d} scaffolding into a Tracking loop that actually
consumes IMU data on every frame and carries the per-keyframe preintegration
forward for a future BA consumer (the backend residual lands in a follow-up
commit so this one is Tracking-only).
Tracking:
- predictVelocityFromImu() preintegrates imu_buffer_ between last_frame_ and
current_frame_ via the Forster integrator. Converts last_frame_'s camera
world pose to the IMU body frame via T_wb = T_wc * T_cam_imu_ before the
integrator runs, so the gravity subtraction and body->world rotation are
done in the right frame. Result goes to current_frame_->velocity_ +
has_velocity_.
- reconcileVelocityWithVisual() blends the IMU prediction with the visual
post-tracking pose delta so Keyframe::velocity_ doesn't accumulate raw
open-loop IMU drift. Blend weight tunable via SVSLAM_VIO_VELOCITY_IMU_ALPHA;
visual-only runs can opt in with SVSLAM_VIO_ENABLE_VISUAL_VELOCITY.
- populateKeyframeImuSpan() attaches a frozen preintegration span between
consecutive KFs (delta_R/v/p, dt, ref biases, from_kf_id, T_cam_imu) to
the new Keyframe::prev_imu_span_, readied for a future BA residual.
- setImuToCameraExtrinsic() + T_cam_imu_ state. Identity by default
preserves existing TUM behavior.
- Gravity-alignment in initializeWithDepth() / initialize() now transforms
the IMU-frame gravity estimate into the camera frame before building the
world-align rotation. Previously this used IMU-frame gravity directly as
if it were in camera frame — invisible on TUM where IMU ≈ camera, but
catastrophic on EuRoC's ~90° offset (world Z-up ended up wrong).
I/O:
- EurocDataset parses cam0 T_BS into cam0_from_imu_ (SE3) with SVD
re-orthonormalization; the YAML reader now folds multi-line "[ ... ]"
arrays so the real EuRoC sensor.yaml with wrapped 16-element matrices
loads correctly.
- apps/run_mono.cc plumbs cam0FromImuExtrinsic() into the tracker when
--accel is active.
Data models:
- ImuPreintegrationSpan holds frozen per-KF-pair preintegration deltas +
the reference biases + T_cam_imu snapshot. Stored on Keyframe as a
unique_ptr; null until populated.
Tests: 2 new EurocDatasetTest cases (T_BS parse from single-line + multi-line
sensor.yaml). TUM regression gates unaffected.
…ion (VIO Stage 0c.c / 0c.a) Consumes the Keyframe::prev_imu_span_ attached by Tracking and turns it into actual BA constraints. With T_cam_imu carried by the span, the residual operates in the correct IMU-body frame without needing extra plumbing from the BA caller. Residuals: - VelocityPreintegrationError: Forster-style 6-DoF (position + velocity) residual between consecutive KFs. Parameter blocks: pose_i, pose_j, vel_i, vel_j, bias_accel_i. Computes p_wb = p_wc - R_wb * t_bc and q_wb = q_wc * q_cb internally so camera-frame BA poses end up in the IMU-body frame where the preintegrated deltas live. First-order accel-bias Jacobians (J_p_ba = -0.5*dt^2*I, J_v_ba = -dt*I) let BA re-estimate the bias without re-integrating. - VelocityDeltaPriorError: kept as a loose fallback for consecutive KF pairs that don't have a valid span (e.g. first few KFs after init). Uses KF::velocity_ as a frozen "position delta ≈ v*dt" prior. - BiasAnchorError: zero-anchor per-bias pull. - BiasRandomWalkError: slow-drift coupling between consecutive KFs. bundleAdjustment: - Per-KF velocity (3d) and accel/gyro bias (3d each) parameter blocks are added when has_velocity_ is set. After the solve they are written back to the corresponding KF fields. - For each consecutive pair (sorted by id), prefers the preintegration residual when kf_j->prev_imu_span_ is valid + from_kf_id matches + dt agrees; falls back to the loose VelocityDeltaPriorError otherwise. - Bias anchor and random-walk residuals shape the bias BA params. Env knobs (all optional): - SVSLAM_BA_VELOCITY_PRIOR_SIGMA_M (pos sigma, default 0.3 m; <=0 disables) - SVSLAM_BA_VELOCITY_PRIOR_VEL_SIGMA (vel sigma, default 0.3 m/s) - SVSLAM_BA_BIAS_ACCEL_ANCHOR_SIGMA (default 0.5 m/s^2) - SVSLAM_BA_BIAS_GYRO_ANCHOR_SIGMA (default 0.1 rad/s) - SVSLAM_BA_BIAS_ACCEL_RW_SIGMA (default 0.05 m/s^2) - SVSLAM_BA_BIAS_GYRO_RW_SIGMA (default 0.005 rad/s) Validation: - All 7 TUM regression gates still pass with bitwise repro (VIO path is dormant on TUM since has_velocity_ stays false). - EuRoC MH_01_easy mono (1500 frames, --accel): ATE mean 0.278 m (visual-only baseline) -> 0.187 m (visual + IMU), median 0.125 m -> 0.069 m after Sim3 alignment with evo_ape. - 71/71 unit tests pass including 5 new residual tests (VelocityPreintegration match + gravity + bias correction, BiasAnchor + BiasRandomWalk).
…ge 0c.d)
Extends VelocityPreintegrationError from 6-DoF (position + velocity) to
full 9-DoF by adding the Forster-style rotation residual
r_rot = log((q_wb_i * delta_R)^{-1} * q_wb_j)
linearized as 2 * (q_err.vec) for small errors. delta_R is sourced from the
Keyframe's ImuPreintegrationSpan and rotated into the body frame via the
same q_cb = q_cam_imu the 6-DoF version already carries, so the rotation
and translation residuals live in one consistent body-in-world frame.
No gyro-bias first-order correction yet — the preintegrated delta_R stays
frozen at the integration-time bias. Gyro bias remains shaped only by its
anchor + random-walk priors.
Env knob: SVSLAM_BA_PREINT_ROT_SIGMA_RAD (default 0.05 rad ≈ 2.9° per
KF-gap). <=0 zeros the rotation weight and restores 6-DoF behavior
without other plumbing changes.
Validation (mono + --accel, Sim3 ATE vs EuRoC ground truth):
- MH_01_easy (3683 frames): 1.728 m -> 1.409 m mean, 1.477 m -> 1.175 m
median, 2.023 m -> 1.779 m rmse (vs prior 6-DoF).
- V1_01_easy (2912 frames): 1.589 m -> 1.466 m mean, 1.584 m -> 1.322 m
median (vs prior 6-DoF). Still +17% over visual-only; closes with VI
Init, which is the next step.
Tests: 2 new OptimizerTest cases (rotation match = zero residual, 10°
mismatch = expected 2*sin(5°) on the Z residual). Residuals array
widened from 6 to 9 across all existing VelocityPreintegrationError
tests. 23/23 VIO-related tests pass.
…c.e)
Adds a VisualInertialInitializer that bootstraps VIO estimate from a
window of the first N keyframes, then refines their state in-place so
the BA preintegration residual (Stage 0c.d) consumes bias-consistent
spans. The initializer is careful to reject its own output when the
visual window is too noisy for a meaningful solve, so MH_01-style
mono sequences fall back cleanly to the pre-Stage-0c.e behaviour.
VisualInertialInitializer (src/tracking/visual_inertial_initializer.{h,cc}):
- Stage 1 — closed-form gyro bias from visual-vs-preintegrated rotation
residuals, with a hard magnitude cap (0.05 rad/s by default). Short
EuRoC windows leave the solve underdetermined, so unregularized output
regularly lands 10× above reality; the cap rejects the runaway and
falls back to zero bias, letting BA's per-KF gyro-bias block take
over. Optional Tikhonov σ exposed but defaults off.
- Stage 2 — LSQ over {scale, gravity_world, per-KF velocities} from the
Forster position + velocity equations. Accel bias is pinned to zero
for this pass (BA's accel-bias parameter + BiasRandomWalkError refine
it later). Scale prior + scale bound guard the degenerate cases.
- Result::applyGyroBiasCorrectionToSpans applies the first-order Forster
rotation correction (delta_R_new = delta_R_ref * Exp(-dbg * dt)) to
every span in the window so BA's Stage 0c.d rotation residual sees a
delta_R consistent with the new reference bias.
- Acceptance gates tightened: rotation_residual_rms_max 0.15 → 0.08 rad
(observed 0.13 rad on noisy MH_01 mono rejects, 0.05 rad on V1_01
passes). Prevents applying a scale/gravity correction when the visual
rotations diverge from the IMU's — that combination produces worse
ATE than doing nothing.
Tracking integration:
- tryVisualInertialInit() runs after each new KF. On success it locks
Map::mutex_ + sets loop_correcting_ to pause Local Mapping, rescales
every KF + landmark + frame pose by the learned scale, rotates the
map so gravity lands on world -Z, writes biases/velocities back to
the window KFs, and calls applyGyroBiasCorrectionToSpans so the BA
rotation residual stays consistent.
- vi_init_done_ flag prevents retry after success.
- On rejection, Tracking falls back silently to the pre-init behavior.
BA gate:
- Optimizer::setPreintegrationResidualEnabled(bool) is kept as a
dataset-level opt-out but flipped to default-on. Gating preint off
until VI init succeeds helps V1_01 (1.31 m → 1.15 m) but punishes
MH_01 where VI init never converges (1.41 m → 3.46 m). With preint
always on, MH_01 matches master and V1_01 still picks up the VI-init
gain. Env knob SVSLAM_VIO_GATE_PREINT=1 restores the gated behavior
for experiments.
Tests: 4 new gtest cases in test_visual_inertial_initializer.cc cover
scale + gravity recovery, capped closed-form gyro bias (with the new
default cap), metric-scale mode, and the missing-span rejection path.
77/77 tests pass.
ATE on EuRoC mono + --accel (Sim3, evo_ape --align --correct_scale):
Dataset visual-only master (0c.d) +VI init (this commit)
MH_01_easy 3.44 m 1.41 m 1.41 m (VI rejected → fallback)
V1_01_easy 1.25 m 1.47 m 1.31 m (VI succeeded, -11%)
No regression on either dataset. V1_01 is the sequence VI init was
designed for; MH_01's mono-init rotations are too noisy for a reliable
linear VI solve, so Stage 0c.e correctly opts out rather than making
things worse.
Follow-ups (not in this commit):
- Re-enable the preint gate only when a dataset-chooser can tell
whether VI init will succeed.
- ORB-SLAM3 Appendix-A style 3-step refinement to make VI init work
on noisier windows (MH_01 recovery).
- First-order gyro-bias Jacobian in the BA rotation residual itself,
so span re-integration isn't the only coupling.
The roadmap section's "IMU tight coupling if ever prioritized" bullet is stale — Stage 0b + 0c landed and is empirically validated on EuRoC MH_01 and V1_01. Update the feature-expansion table, add a VIO status subsection with the commit range, ATE improvements, and the env-knob defaults we converged on, plus a note about the gyro-bias Jacobian experiment that was tried and reverted (regressed MH_01 2–3× because BA absorbs visual rotation noise into the bias when biases are uncalibrated).
std::rand() isn't seeded, so every fresh process starts at the same sequence (1804289383, ...). When ctest -j launches the test binary in parallel with different --gtest_filters they can land on the same /tmp/svslam_euroc_test_<N> directory and one of them fails with \"no entries in data.csv\" or a filesystem race on the PNG writes. Swap the counter for (PID + process-local atomic counter + nanosecond timestamp). Ran ctest -R EurocDatasetTest -j4 five times in a row with no failures after the change.
…2.0) Add SVSLAM_BA_GRAVITY_PRIOR_WEIGHT so aggressive-motion sequences can disable the per-KF gravity prior when the ±50 ms accel window is dominated by motion rather than gravity. Default 2.0 preserves the Stage 0c behavior for MH_01 / MH_02 where the prior demonstrably helps. MH_03_medium empirically neither gains nor loses from the prior (2.91 m with, 2.90 m without) — the knob mostly exists to cut the experimentation loop short when probing a new dataset.
Owner
Author
Cross-sequence validation (2026-04-20)Validated on five EuRoC mono +
Observations:
No TUM regression: 7/7 TUM gates still pass with bitwise-identical trajectories. |
Rewrites the stale chunks of plan.md for a clean handoff to the next agent — specifically so a Codex pickup has an accurate current-state picture and a VIO-aware guidance path. Major changes: - Header updates date to 2026-04-21 and explicitly calls out the VIO Stage 0b / 0c landing on master. - §2.1 Snapshot: replaces stale HEAD / test-count / LOC numbers with current values (HEAD bba6791 on master, 77/77 tests, 12 090 LOC, 18 test files). Adds PR #3 branch pointer. - §2.2 Feature Matrix: adds the loosely-coupled VIO row, flags the known-degraded EuRoC stereo path, notes ROS2 has no VIO hookup yet. - §2.4 Regression Gates: lists all 7 gates (the 2 _accel_head_repro variants were missing) with fresh numbers and thin-margin call-outs. - §2.7: replaces the synthetic-dataset section with real-dataset verification status (5 EuRoC sequences on disk via Wayback, the GT TUM conversion one-liner, evo_ape recipe). - §2.8: new — EuRoC mono --accel VIO sweep table (MH_01/02/03, V1_01, V2_01) with Δ mean / Δ max / VI-init accept status and the σ sweep finding. - §3: adds the 10 VIO commits (Stage 0b.a through 0c.e) with per-stage labels, plus the 2 extra vio-integration-branch commits. - §4.2: updates file-inventory entries for src/tracking, src/backend, src/sensors, src/io, tests/ to reflect the new VIO sources and updated LOC estimates. - §7.8: new — complete VIO env-knob table (12 knobs) with defaults and tuning philosophy. - §8 Known Issues: rewrites the list — marks "real EuRoC missing" done, adds MH_03 regression (§8.11), the gyro-bias-Jacobian attempt that reverted (§8.10), the ORB 3000 regression that proved plan.md §10.4 is partly stale (§8.12), and the EuRoC stereo rectification issue (§8.13). - §9 Phase D: expanded status with the VIO sub-table and ORB-SLAM3-follow-up pointers. - §10.4 #1: flags as tested-and-regressed with the measured deltas. - §11: new priority order — land PR #3, diagnose MH_03, write an eval/ artifact for the EuRoC sweep, resume §10 probes. - §12 AI Agent Instructions: expands with VIO-aware reading order, the "narrowest gate first" rule per-topic, and explicit warnings about the gyro-bias Jacobian + plan.md §10 staleness. - §15: marks the Mono Room Handoff as historical with a header note. - §16: new — VIO Handoff section with pipeline diagram, key invariants, MH_03 bisection plan, Stage 0c.f follow-ups, and minimum reference-file list for any VIO change. - §17: Non-Goals renumbered from §16. Net delta: 1705 lines (previously 1418). Preserves §14 battle log and §15 mono-room diagnostic context since the guidance inside them is still relevant for any mono front-end change.
rsasaki0109
added a commit
that referenced
this pull request
Apr 20, 2026
Rewrites the stale chunks of plan.md for a clean handoff to the next agent — specifically so a Codex pickup has an accurate current-state picture and a VIO-aware guidance path. Major changes: - Header updates date to 2026-04-21 and explicitly calls out the VIO Stage 0b / 0c landing on master. - §2.1 Snapshot: replaces stale HEAD / test-count / LOC numbers with current values (HEAD 46a726d on master, 77/77 tests, 12 090 LOC, 18 test files). Adds PR #3 branch pointer. - §2.2 Feature Matrix: adds the loosely-coupled VIO row, flags the known-degraded EuRoC stereo path, notes ROS2 has no VIO hookup yet. - §2.4 Regression Gates: lists all 7 gates (the 2 _accel_head_repro variants were missing) with fresh numbers and thin-margin call-outs. - §2.7: replaces the synthetic-dataset section with real-dataset verification status (5 EuRoC sequences on disk via Wayback, the GT TUM conversion one-liner, evo_ape recipe). - §2.8: new — EuRoC mono --accel VIO sweep table (MH_01/02/03, V1_01, V2_01) with Δ mean / Δ max / VI-init accept status and the σ sweep finding. - §3: adds the 10 VIO commits (Stage 0b.a through 0c.e) with per-stage labels, plus the 2 extra vio-integration-branch commits. - §4.2: updates file-inventory entries for src/tracking, src/backend, src/sensors, src/io, tests/ to reflect the new VIO sources and updated LOC estimates. - §7.8: new — complete VIO env-knob table (12 knobs) with defaults and tuning philosophy. - §8 Known Issues: rewrites the list — marks "real EuRoC missing" done, adds MH_03 regression (§8.11), the gyro-bias-Jacobian attempt that reverted (§8.10), the ORB 3000 regression that proved plan.md §10.4 is partly stale (§8.12), and the EuRoC stereo rectification issue (§8.13). - §9 Phase D: expanded status with the VIO sub-table and ORB-SLAM3-follow-up pointers. - §10.4 #1: flags as tested-and-regressed with the measured deltas. - §11: new priority order — land PR #3, diagnose MH_03, write an eval/ artifact for the EuRoC sweep, resume §10 probes. - §12 AI Agent Instructions: expands with VIO-aware reading order, the "narrowest gate first" rule per-topic, and explicit warnings about the gyro-bias Jacobian + plan.md §10 staleness. - §15: marks the Mono Room Handoff as historical with a header note. - §16: new — VIO Handoff section with pipeline diagram, key invariants, MH_03 bisection plan, Stage 0c.f follow-ups, and minimum reference-file list for any VIO change. - §17: Non-Goals renumbered from §16. Net delta: 1705 lines (previously 1418). Preserves §14 battle log and §15 mono-room diagnostic context since the guidance inside them is still relevant for any mono front-end change.
rsasaki0109
added a commit
that referenced
this pull request
Jun 17, 2026
Rewrites the stale chunks of plan.md for a clean handoff to the next agent — specifically so a Codex pickup has an accurate current-state picture and a VIO-aware guidance path. Major changes: - Header updates date to 2026-04-21 and explicitly calls out the VIO Stage 0b / 0c landing on master. - §2.1 Snapshot: replaces stale HEAD / test-count / LOC numbers with current values (HEAD 52f08da on master, 77/77 tests, 12 090 LOC, 18 test files). Adds PR #3 branch pointer. - §2.2 Feature Matrix: adds the loosely-coupled VIO row, flags the known-degraded EuRoC stereo path, notes ROS2 has no VIO hookup yet. - §2.4 Regression Gates: lists all 7 gates (the 2 _accel_head_repro variants were missing) with fresh numbers and thin-margin call-outs. - §2.7: replaces the synthetic-dataset section with real-dataset verification status (5 EuRoC sequences on disk via Wayback, the GT TUM conversion one-liner, evo_ape recipe). - §2.8: new — EuRoC mono --accel VIO sweep table (MH_01/02/03, V1_01, V2_01) with Δ mean / Δ max / VI-init accept status and the σ sweep finding. - §3: adds the 10 VIO commits (Stage 0b.a through 0c.e) with per-stage labels, plus the 2 extra vio-integration-branch commits. - §4.2: updates file-inventory entries for src/tracking, src/backend, src/sensors, src/io, tests/ to reflect the new VIO sources and updated LOC estimates. - §7.8: new — complete VIO env-knob table (12 knobs) with defaults and tuning philosophy. - §8 Known Issues: rewrites the list — marks "real EuRoC missing" done, adds MH_03 regression (§8.11), the gyro-bias-Jacobian attempt that reverted (§8.10), the ORB 3000 regression that proved plan.md §10.4 is partly stale (§8.12), and the EuRoC stereo rectification issue (§8.13). - §9 Phase D: expanded status with the VIO sub-table and ORB-SLAM3-follow-up pointers. - §10.4 #1: flags as tested-and-regressed with the measured deltas. - §11: new priority order — land PR #3, diagnose MH_03, write an eval/ artifact for the EuRoC sweep, resume §10 probes. - §12 AI Agent Instructions: expands with VIO-aware reading order, the "narrowest gate first" rule per-topic, and explicit warnings about the gyro-bias Jacobian + plan.md §10 staleness. - §15: marks the Mono Room Handoff as historical with a header note. - §16: new — VIO Handoff section with pipeline diagram, key invariants, MH_03 bisection plan, Stage 0c.f follow-ups, and minimum reference-file list for any VIO change. - §17: Non-Goals renumbered from §16. Net delta: 1705 lines (previously 1418). Preserves §14 battle log and §15 mono-room diagnostic context since the guidance inside them is still relevant for any mono front-end change.
rsasaki0109
force-pushed
the
vio-integration
branch
from
5fa20e2 to
0091eb4
Compare
rsasaki0109
added a commit
that referenced
this pull request
Jun 17, 2026
Rewrites the stale chunks of plan.md for a clean handoff to the next agent — specifically so a Codex pickup has an accurate current-state picture and a VIO-aware guidance path. Major changes: - Header updates date to 2026-04-21 and explicitly calls out the VIO Stage 0b / 0c landing on master. - §2.1 Snapshot: replaces stale HEAD / test-count / LOC numbers with current values (HEAD fcf8926 on master, 77/77 tests, 12 090 LOC, 18 test files). Adds PR #3 branch pointer. - §2.2 Feature Matrix: adds the loosely-coupled VIO row, flags the known-degraded EuRoC stereo path, notes ROS2 has no VIO hookup yet. - §2.4 Regression Gates: lists all 7 gates (the 2 _accel_head_repro variants were missing) with fresh numbers and thin-margin call-outs. - §2.7: replaces the synthetic-dataset section with real-dataset verification status (5 EuRoC sequences on disk via Wayback, the GT TUM conversion one-liner, evo_ape recipe). - §2.8: new — EuRoC mono --accel VIO sweep table (MH_01/02/03, V1_01, V2_01) with Δ mean / Δ max / VI-init accept status and the σ sweep finding. - §3: adds the 10 VIO commits (Stage 0b.a through 0c.e) with per-stage labels, plus the 2 extra vio-integration-branch commits. - §4.2: updates file-inventory entries for src/tracking, src/backend, src/sensors, src/io, tests/ to reflect the new VIO sources and updated LOC estimates. - §7.8: new — complete VIO env-knob table (12 knobs) with defaults and tuning philosophy. - §8 Known Issues: rewrites the list — marks "real EuRoC missing" done, adds MH_03 regression (§8.11), the gyro-bias-Jacobian attempt that reverted (§8.10), the ORB 3000 regression that proved plan.md §10.4 is partly stale (§8.12), and the EuRoC stereo rectification issue (§8.13). - §9 Phase D: expanded status with the VIO sub-table and ORB-SLAM3-follow-up pointers. - §10.4 #1: flags as tested-and-regressed with the measured deltas. - §11: new priority order — land PR #3, diagnose MH_03, write an eval/ artifact for the EuRoC sweep, resume §10 probes. - §12 AI Agent Instructions: expands with VIO-aware reading order, the "narrowest gate first" rule per-topic, and explicit warnings about the gyro-bias Jacobian + plan.md §10 staleness. - §15: marks the Mono Room Handoff as historical with a header note. - §16: new — VIO Handoff section with pipeline diagram, key invariants, MH_03 bisection plan, Stage 0c.f follow-ups, and minimum reference-file list for any VIO change. - §17: Non-Goals renumbered from §16. Net delta: 1705 lines (previously 1418). Preserves §14 battle log and §15 mono-room diagnostic context since the guidance inside them is still relevant for any mono front-end change.
rsasaki0109
force-pushed
the
vio-integration
branch
from
0091eb4 to
ddd298a
Compare
rsasaki0109
force-pushed
the
vio-integration
branch
from
ddd298a to
baa8b88
Compare
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.
Summary
Validated ATE (evo_ape --align --correct_scale --t_max_diff 0.05)
MH_01 rejects VI init (visual rotations too noisy for the 0.08 rad threshold) and benefits from the 9-DoF BA residual alone. V1_01 accepts VI init and picks up additional scale + gravity refinement.
Env knobs (all optional)
Test plan
Not in this PR