diff --git a/CMakeLists.txt b/CMakeLists.txt index 84ed66b..b91d4fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -189,6 +189,7 @@ add_library(svslam_core src/io/map_io.cc src/tracking/tracking.cc src/tracking/initializer.cc + src/tracking/visual_inertial_initializer.cc src/backend/local_mapping.cc src/backend/optimizer.cc src/loop_closing/loop_closing.cc @@ -266,6 +267,8 @@ if(BUILD_TESTS) tests/test_tum_pinhole_calibration.cc tests/test_tracking_pose_recompute.cc tests/test_tracking_run_statistics.cc + tests/test_imu_preintegrator.cc + tests/test_visual_inertial_initializer.cc ) if(USE_DEPTH_DL) target_sources(svslam_tests PRIVATE tests/test_metric_depth_estimator.cc) diff --git a/apps/run_mono.cc b/apps/run_mono.cc index 51b7d7f..fcf3c13 100644 --- a/apps/run_mono.cc +++ b/apps/run_mono.cc @@ -19,6 +19,7 @@ #include "io/tum_dataset.h" #include "io/tum_pinhole_calibration.h" #include "io/map_io.h" +#include "backend/optimizer.h" #include "core/heuristic_reference_keyframe_policy.h" #include "tracking/tracking.h" #include "backend/local_mapping.h" @@ -409,6 +410,46 @@ int main(int argc, char** argv) { } } + if (use_euroc) { + if (use_accel && euroc.hasImu()) { + // Mirror the IMU accel channel into accel_buffer_ so the existing + // gravity-alignment + stationary-detection paths work unchanged, + // and retain the full IMU (accel + gyro) for future VIO use. + tracker->imu_buffer_ = euroc.allImu(); + tracker->accel_buffer_.clear(); + tracker->accel_buffer_.reserve(tracker->imu_buffer_.size()); + for (const auto& imu : tracker->imu_buffer_) { + AccelEntry accel; + accel.timestamp_sec = imu.timestamp_sec; + accel.ax = imu.accel.x(); + accel.ay = imu.accel.y(); + accel.az = imu.accel.z(); + tracker->accel_buffer_.push_back(accel); + } + if (euroc.hasCam0FromImuExtrinsic()) { + tracker->setImuToCameraExtrinsic(euroc.cam0FromImuExtrinsic()); + } + // VIO Stage 0c.e: keep the IMU preintegration residual on by + // default so sequences where VI init rejects (e.g. MH_01 with + // noisy early-mono rotations) still benefit from the 9-DoF + // residual + BA bias blocks — the regression we feared from + // "preint on with bias=0" never materialises on MH/V1 runs in + // practice (loose anchor + RW priors soak up the initial + // mis-estimate). Set SVSLAM_VIO_GATE_PREINT=1 to opt back in to + // gating preint until VI init succeeds (useful when diagnosing + // scale-sensitive sequences). + if (std::getenv("SVSLAM_VIO_GATE_PREINT")) { + Optimizer::setPreintegrationResidualEnabled(false); + } + std::cout << "IMU integration: ENABLED (accel+gyro, " + << tracker->imu_buffer_.size() << " samples)" << std::endl; + } else if (use_accel) { + std::cout << "IMU integration: requested but mav0/imu0/data.csv absent, DISABLED" + << std::endl; + use_accel = false; + } + } + // Deep learning depth estimator if (loop_closing && use_depth) { loop_closing->setMetricDepth(true); diff --git a/plan.md b/plan.md index 25e253f..aefc868 100644 --- a/plan.md +++ b/plan.md @@ -1,19 +1,23 @@ # SimpleVisualSLAM Agent Handoff -This file is the authoritative handoff for Codex / Claude / Cursor as of **2026-04-15**. -It was rewritten from scratch using current command output from this workspace: +This file is the authoritative handoff for Codex / Claude / Cursor as of **2026-04-21**. -- `git log --oneline -20` -- `git diff --stat HEAD~16..HEAD` -- `ctest --test-dir build_codex --output-on-failure` +Major update on this revision: **VIO Stage 0b + 0c landed.** A loosely-coupled visual-inertial pipeline (IMU preintegration in local BA, per-KF velocity + accel/gyro bias BA parameters, Forster 9-DoF preintegration residual, linear Visual-Inertial Initialization) is live on `master` and empirically validated on five EuRoC mono `--accel` sequences. See §16 for the VIO-specific handoff. + +Reconnaissance commands used for this revision: + +- `git log --oneline -25` +- `git diff --stat 3f9bc71..HEAD` (span since the last merged PR) +- `ctest --test-dir build --output-on-failure` (77/77 PASS) +- `python3 scripts/check_regression_gate.py --build build --all-gates --quiet` (7/7 PASS) +- `cat eval/regression_baselines.json` - `cat eval/stella_comparison_results.md` - `cat eval/metric_depth_test_results.md` - `cat eval/euroc_test_results.md` -- `wc -l src/**/*.cc src/**/*.h apps/*.cc` -- `ls tests/test_*.cc` -- `cat eval/regression_baselines.json` -- `./build_codex/run_mono --version` -- `./build_codex/run_mono --help` +- `wc -l apps/*.cc src/*/*.cc src/*/*.h` → 12 090 lines +- `ls tests/test_*.cc` → 18 files +- `./build/run_mono --version` / `--help` +- EuRoC mono + `--accel` 5-sequence ATE sweep: MH_01/02/03, V1_01, V2_01 (Sim3 evo_ape) If this document and the code disagree, the code is correct and this file should be updated. @@ -41,21 +45,24 @@ What the project is trying to become: --- -## 2. Current State (2026-04-16) +## 2. Current State (2026-04-21) ### 2.1 Snapshot | Item | Current value | | --- | --- | -| HEAD | run `git rev-parse --short HEAD` | -| HEAD subject | `Improve mono room ATE; pose graph backbone; diagnostics` | +| `master` HEAD | `46a726d` | +| `master` HEAD subject | `plan.md: record VIO Stage 0b/0c progress and validated ATE numbers` | +| Open PR branch | `vio-integration` (`8e6f4e7`), **PR #3** on GitHub, not yet merged | | Version | `SimpleVisualSLAM 0.2.0` | -| Build used for this snapshot | `build_codex` | -| Recent change volume | `73 files changed, 7646 insertions(+), 1269 deletions(-)` in `HEAD~16..HEAD` | -| Unit tests | `58/58` passed | -| `ctest` wall time | `6.94 sec` | -| Core app/source LOC | `9715` lines across `apps/*.cc` + requested `src/**/*.cc` + `src/**/*.h` | -| Test source files | `16` `tests/test_*.cc` files | +| Build used for this snapshot | `build` (no suffix — the Ninja-style build dirs `build_codex*` are legacy) | +| Recent change volume | `22 files changed, 2 820 insertions(+), 65 deletions(-)` in `3f9bc71..HEAD` (the span that introduced the whole VIO pipeline) | +| Unit tests | **77 / 77** passed | +| `ctest` wall time | `~10 sec` | +| Core app/source LOC | **12 090** lines across `apps/*.cc` + `src/*/*.cc` + `src/*/*.h` | +| Test source files | **18** `tests/test_*.cc` files | +| TUM regression gates | **7 / 7** PASS with bitwise-reproducible trajectories | +| EuRoC mono `--accel` sweep (5 sequences, 2026-04-20) | average ATE **2.10 m → 1.60 m (−24 %)** vs visual-only | ### 2.2 Supported Feature Matrix @@ -63,10 +70,11 @@ What the project is trying to become: | --- | --- | --- | | Monocular SLAM | Implemented | `run_mono --tum ...` and default video path | | RGB-D SLAM | Implemented | `--depth` | -| Accelerometer prior | Implemented | `--accel`; gravity prior in BA | -| EuRoC mono loader | Implemented | `--euroc ` | -| EuRoC stereo depth | Implemented | `--euroc ... --stereo` | -| Stereo tracking mode | Partial | stereo depth is computed from `cam0+cam1`, but tracking still runs on `cam0` | +| Accelerometer prior (BA gravity) | Implemented | `--accel`; per-KF gravity prior, weight env-tunable via `SVSLAM_BA_GRAVITY_PRIOR_WEIGHT` | +| **Loosely-coupled VIO (Stage 0b + 0c)** | **Implemented** | IMU preintegration (`sensors/imu_preintegrator.h`), BA preintegration residual (`VelocityPreintegrationError`, 9-DoF) with accel/gyro bias blocks, Forster rotation residual, Visual-Inertial Initialization (`tracking/visual_inertial_initializer.{h,cc}`). Active whenever `--accel` is paired with an IMU dataset (EuRoC) | +| EuRoC mono + IMU loader | Implemented | `--euroc --accel`; loads `cam0 T_BS` multi-line YAML, parses `imu0/data.csv`, plumbs extrinsic into Tracking | +| EuRoC stereo depth | Implemented, known-degraded | `--euroc ... --stereo`; stereo-only ATE on MH_01 is ~2.77 m vs 3.44 m mono — rectification pipeline does not handle EuRoC's fisheye-ish distortion well. Separate from the VIO work, see §8 | +| Stereo tracking mode | Partial | stereo depth is computed from `cam0+cam1`, tracking still runs on `cam0` | | Relative DL depth | Implemented | `--depth-model ` with `-DUSE_DEPTH_DL=ON` | | Metric DL depth | Implemented | `--metric-depth-model ` with `-DUSE_DEPTH_DL=ON` | | Loop closing | Implemented | enabled in async runs when ORB vocabulary exists | @@ -74,7 +82,7 @@ What the project is trying to become: | Map persistence | Implemented | writes `map.bin`, `trajectory.txt`, `trajectory_online.txt`, `trajectory_keyframes.txt` | | Run summary JSON | Implemented | `--run-summary-json ` | | Strict failure exit | Implemented | `--strict-exit` | -| ROS2 Jazzy node | Implemented, basic | `ros2/src/slam_node.cc`; currently no loop-closing parity | +| ROS2 Jazzy node | Implemented, basic | `ros2/src/slam_node.cc`; currently no loop-closing parity, no VIO hookup | ### 2.3 CLI Surface @@ -103,22 +111,24 @@ The ORB vocabulary is the last positional argument, otherwise the app searches ` ### 2.4 Regression Gates -These are the **current measured values** from this workspace on **2026-04-16** (`python3 -u scripts/check_regression_gate.py --build build_codex --all-gates --quiet`). +Measured on **2026-04-21** (`python3 scripts/check_regression_gate.py --build $PWD/build --data-tum $PWD/data/tum --all-gates --quiet`). | Gate | Mode | Mean ATE (m) | Ceiling (m) | Status | | --- | --- | ---: | ---: | --- | -| `room_depth_accel_head_repro` | RGB-D + accel | `0.057702` | `0.145000` | PASS | -| `room_depth_head_repro` | RGB-D | `0.079914` | `0.165000` | PASS | -| `room_mono_head_repro` | Mono | `0.197374` | `0.340000` | PASS | | `xyz_depth_head_repro` | RGB-D | `0.011042` | `0.016000` | PASS | | `xyz_mono_head_repro` | Mono | `0.028136` | `0.030000` | PASS | +| `xyz_mono_accel_head_repro` | Mono + accel | `0.024931` | `0.027000` | PASS | +| `room_depth_head_repro` | RGB-D | `0.079914` | `0.165000` | PASS | +| `room_depth_accel_head_repro` | RGB-D + accel | `0.057702` | `0.145000` | PASS | +| `room_mono_head_repro` | Mono | `0.176506` | `0.340000` | PASS | +| `room_mono_accel_head_repro` | Mono + accel | `0.164001` | `0.170000` | PASS (thin margin) | -Current gate summary on `build_codex`: - -- **5/5 gates passing** -- **Current mono focus:** `room_mono_head_repro` is still the weakest head-250 gate by a wide margin +**7 / 7 gates passing** with bitwise-identical trajectories on two back-to-back runs per gate. -This is the most important delta versus older planning notes. The earlier `xyz_mono_head_repro` blocker has been recovered on the current build, but the pass margin is still thin and `room_mono` remains far from the external baseline. +Notes: +- VIO's preintegration residual is dormant on TUM runs (no `has_velocity_`) so these gates measure the non-VIO front-end/back-end behaviour — VIO work did not regress any gate. +- `room_mono_accel_head_repro` has the smallest pass margin (6 mm). Any mono front-end change must re-run this gate before and after. +- `room_mono_head_repro` is still the widest gap to `stella_vslam` (§2.5 / §10.4). ### 2.5 `stella_vslam` Comparison Status @@ -180,29 +190,72 @@ Interpretation: - `xyz` looks promising - `room` at `250` frames is still poor, and loop candidates were rejected by `computeSim3()` -### 2.7 EuRoC Stereo Verification Status +### 2.7 EuRoC Real-Dataset Verification (Updated 2026-04-20) + +Real EuRoC sequences are now on disk under `datasets/euroc/` — downloaded via the **Wayback Machine** because the ETH ASL origin (`robotics.ethz.ch/~asl-datasets/...`) is unreachable from this environment. The working recipe: + +``` +curl -L --fail --retry 3 -o datasets/euroc/.zip \ + "https://web.archive.org/web/if_/http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset//.zip" +``` + +The `if_` modifier on the Wayback URL returns the raw ZIP rather than the framed viewer. Find a valid `` with `curl -sSI "https://web.archive.org/web/2023/..."` and follow the 302 redirect. Typical capture size ≈ original (1–1.6 GB), download ≈ 4 min at ~6 MB/s. + +Sequences available on disk: + +| Sequence | ZIP (GB) | Cam frames | IMU samples | GT poses | Ground truth file | +| --- | ---: | ---: | ---: | ---: | --- | +| `MH_01_easy` | 1.57 | 3 683 | 36 820 | 36 382 | `datasets/euroc/MH_01_easy/gt_tum.txt` | +| `MH_02_easy` | 1.29 | 3 041 | 30 400 | 29 993 | `datasets/euroc/MH_02_easy/gt_tum.txt` | +| `MH_03_medium` | 1.11 | 2 701 | 27 008 | 26 302 | `datasets/euroc/MH_03_medium/gt_tum.txt` | +| `V1_01_easy` | 1.15 | 2 912 | 29 120 | 28 711 | `datasets/euroc/V1_01_easy/gt_tum.txt` | +| `V2_01_easy` | 0.88 | 2 281 | 22 800 | 22 401 | `datasets/euroc/V2_01_easy/gt_tum.txt` | + +GT → TUM conversion (one-liner, EuRoC ns + `[qw,qx,qy,qz]` → TUM s + `[qx,qy,qz,qw]`): + +``` +awk -F',' 'NR>1 { printf "%.9f %s %s %s %s %s %s %s\n", $1/1e9, $2, $3, $4, $6, $7, $8, $5 }' \ + /mav0/state_groundtruth_estimate0/data.csv > /gt_tum.txt +``` + +ATE evaluation uses Sim(3) alignment (mono scale is unobservable without explicit VIO metric output): -From `eval/euroc_test_results.md`: +``` +evo_ape tum /gt_tum.txt build/trajectory.txt --align --correct_scale --t_max_diff 0.05 --no_warnings +``` + +Run flags that worked: + +- Mono VIO: `./build/run_mono --euroc datasets/euroc/ --accel --reference-policy heuristic --skip-frames 0 --no-viz --repro-eval` +- Visual-only baseline: same command minus `--accel`. + +stereo baseline auto-loaded from sensor.yaml: `0.110074 m` on MH_01. +Passing the inner `mav0` directory is wrong for this loader (`EurocDataset` already appends `/mav0/...`). + +### 2.8 EuRoC Mono `--accel` VIO Sweep (2026-04-20) -- The legacy per-sequence EuRoC download did **not** succeed from this environment on 2026-04-14 -- The current official distribution path points to an ETH Research Collection DOI and a large combined Machine Hall bundle rather than a direct `MH_01_easy.zip` -- The repo therefore used a **synthetic EuRoC-style fallback dataset** under `data/euroc/test_seq` +Measured on the five sequences above, full length (no `--max-frames`), Sim(3) alignment via `evo_ape`. -Verification results: +| Sequence | Frames | Visual only | +VIO | Δ mean | Δ max | VI init | +| --- | ---: | ---: | ---: | ---: | ---: | --- | +| `MH_01_easy` | 3 683 | 3.44 m | **1.41 m** | **−59 %** | −43 % | rejected (rot_rms 0.13) | +| `MH_02_easy` | 3 041 | 2.64 m | **1.72 m** | **−35 %** | −42 % | rejected | +| `MH_03_medium` | 2 701 | **2.48 m** | 2.91 m | **+17 %** | +59 % | rejected | +| `V1_01_easy` | 2 912 | 1.25 m | **1.31 m** | +5 % | **−59 %** | **succeeded** (scale 0.98) | +| `V2_01_easy` | 2 281 | 0.69 m | **0.63 m** | **−9 %** | **−53 %** | rejected (scale outside tolerance) | +| **Average** | | **2.10 m** | **1.60 m** | **−24 %** | — | 1 / 5 accept | -| Mode | Frames | Final state | Keyframes | Landmarks | Status | -| --- | ---: | ---: | ---: | ---: | --- | -| EuRoC mono | `10` | `2` (`OK`) | `3` | `277` | PASS | -| EuRoC stereo | `10` | `2` (`OK`) | `4` | `1223` | PASS | +Read these numbers as: -Extra facts worth keeping in mind: +- **Machine Hall (long flight, translation-dominant):** IMU wins decisively on MH_01/02 because visual drift accumulates over distance and IMU anchors orientation. MH_03 is the open regression (§8.11). +- **Vicon Room (short, rotation-heavy):** the mean barely moves but the worst-case excursion (`max`) drops 50–60 %, i.e. IMU kills the occasional tracking runaway. +- **VI init acceptance is the second-order story, not the first.** The 9-DoF preintegration residual alone delivers the bulk of the gain. VI init just adds a small scale + gravity refinement on the one sequence (V1_01) whose early-mono rotations are clean enough to pass the 0.08 rad residual threshold. -- stereo baseline loaded: `0.110074 m` -- successful command uses the **sequence root**: - - `build_codex/run_mono --euroc data/euroc/test_seq ...` -- passing the inner `mav0` directory is wrong for this loader and fails because `EurocDataset` already appends `/mav0/...` +Machine-readable artifact: none yet — the numbers above came from ad-hoc runs during the 0c.e validation. A proper `eval/euroc_vio_sweep.md` / `.json` is a pending artifact (§11). -### 2.8 600-Frame Loop Stability +Knob sweep finding (MH_01 only): `SVSLAM_BA_VELOCITY_PRIOR_SIGMA_M` ∈ {0.1, 0.3, 0.5, 1.0}, σ=0.3 is optimal (1.73 m vs 3.70 / 2.02 / 2.05 m). Tighter over-constrains; looser wastes IMU signal. σ=0.3 is the default. + +### 2.9 600-Frame Loop Stability (historical) There are two different pieces of evidence in the repo, and they should not be conflated: @@ -232,7 +285,31 @@ Current truth: ## 3. Commit History -`git diff --stat HEAD~16..HEAD` spans the last 16 commits and shows the overall churn. The handoff history below starts at the requested anchor `d3c81a7` and covers the 15 commits from there to current HEAD. +Two spans matter for this handoff. + +**Span A — since last merged PR (`3f9bc71` → `master` HEAD `46a726d`):** 10 commits introducing VIO Stage 0b + 0c. Subsequent cleanup commits (`688f748`, `8e6f4e7`) live on the `vio-integration` branch / PR #3. + +| Commit | Stage | What changed | +| --- | --- | --- | +| `38073b6` | 0b.b | `core/` velocity + IMU-bias scaffolding on `Frame` / `Keyframe` | +| `2f9bbf7` | 0b.a | `sensors/imu_preintegrator.h` (Forster-style skeleton) | +| `3b90a77` | 0b.d | `ImuEntry` type + EuRoC `imu0` loader | +| `03bd424` | 0b.c | Plumb EuRoC IMU into Tracking via `imu_buffer_` | +| `06868c5` | pre-0c | Guard `Frame::landmarks_` and `Keyframe::landmarks_` container races | +| `bebc7d5` | 0b.e / 0b.f / 0c.b | Tracking-side IMU path (`predictVelocityFromImu`, `reconcileVelocityWithVisual`), EuRoC `cam0 T_BS` extrinsic, mono/depth init gravity transform into camera frame | +| `f9303ac` | 0c.c / 0c.a | `VelocityPreintegrationError` (Forster 6-DoF pos + vel), per-KF velocity + accel/gyro bias BA parameter blocks, `BiasAnchorError` + `BiasRandomWalkError` | +| `935a33c` | 0c.d | 3-DoF rotation residual added (9-DoF total), `delta_R` kept frozen (no gyro Jacobian yet) | +| `bd7b691` | 0c.e | `VisualInertialInitializer` (linear 2-stage solve), `tryVisualInertialInit` in Tracking, `applyGyroBiasCorrectionToSpans`, acceptance thresholds (rot_rms ≤ 0.08 rad, gyro-bias cap 0.05 rad/s) | +| `46a726d` | docs | `plan.md`: record Stage 0b/0c progress (this file was the previous update) | + +On `vio-integration` branch (PR #3): + +| Commit | What changed | +| --- | --- | +| `688f748` | `tests/test_euroc_dataset.cc`: unique temp paths across `ctest -j` workers (PID + atomic counter + nanosecond timestamp) | +| `8e6f4e7` | `backend/optimizer.cc`: env-tunable gravity prior weight (`SVSLAM_BA_GRAVITY_PRIOR_WEIGHT`, default 2.0) | + +**Span B — earlier context preserved for archaeology:** | Commit | What changed | | --- | --- | @@ -303,15 +380,17 @@ These are outside the directory inventory requested below but matter immediately - `initializer.cc` (`557`): monocular two-view initialization, H/F model selection, triangulation - `initializer.h` (`42`): initializer API and result containers -- `tracking.cc` (`2116`): front-end tracking, motion model, keyframe decision, local-map tracking, relocalization, reinitialization, loop-correction handoff -- `tracking.h` (`149`): tracking state, recovery state, loop-correction state, run statistics, thresholds +- `tracking.cc` (`~2720`): front-end tracking, motion model, keyframe decision, local-map tracking, relocalization, reinitialization, loop-correction handoff, **IMU velocity prediction (`predictVelocityFromImu`), visual-velocity reconciliation (`reconcileVelocityWithVisual`), preintegration span attachment (`populateKeyframeImuSpan`), VI init entry (`tryVisualInertialInit`)** +- `tracking.h` (`~200`): tracking state, recovery state, loop-correction state, run statistics, thresholds, **`T_cam_imu_` extrinsic, VI init bookkeeping** +- **`visual_inertial_initializer.cc` (`~400`):** linear two-stage VI init (closed-form gyro bias with cap, LSQ for scale + gravity + velocities, span delta_R first-order correction via `applyGyroBiasCorrectionToSpans`) +- **`visual_inertial_initializer.h` (`~165`):** `Options` / `Result` structs, acceptance thresholds #### `src/backend/` - `local_mapping.cc` (`449`): local-mapping queue, new-point creation, map-point culling, local BA - `local_mapping.h` (`62`): local-mapping API, queue, callback hook -- `optimizer.cc` (`980`): pose-only PnP refinement, local BA, depth prior, gravity prior, pose graph, IRLS -- `optimizer.h` (`113`): Ceres residual definitions and optimizer API +- `optimizer.cc` (`~1250`): pose-only PnP refinement, local BA, depth prior, gravity prior (env-tunable weight), pose graph, IRLS, **VIO preintegration residual wiring (velocity + accel/gyro bias blocks, bias anchor + random-walk priors, preint-residual gate for VI-init-aware datasets)** +- `optimizer.h` (`~390`): Ceres residual definitions and optimizer API, **`VelocityPreintegrationError` (Forster 9-DoF pos + vel + rot with first-order accel-bias Jacobian), `VelocityDeltaPriorError` (loose fallback), `BiasAnchorError`, `BiasRandomWalkError`** #### `src/loop_closing/` @@ -331,11 +410,14 @@ These are outside the directory inventory requested below but matter immediately #### `src/sensors/` - `accelerometer.h` (`80`): accelerometer entry type and simple processing helpers +- `imu.h` (`15`): `ImuEntry` (accel + gyro + timestamp) value type +- `imu_preintegrator.h` (`~110`): Forster-style IMU preintegration (`deltaR/deltaV/deltaP/dt`, bias reset, `integrate`, `predict` with gravity) +- `imu_preintegration_span.h` (`~40`): frozen per-KF-pair preintegration span (deltas, reference biases, `T_cam_imu` snapshot, `from_kf_id`, validity) #### `src/io/` -- `euroc_dataset.cc` (`427`): EuRoC dataset loader, stereo pairing, calibration setup, baseline extraction -- `euroc_dataset.h` (`78`): EuRoC dataset API +- `euroc_dataset.cc` (`~495`): EuRoC dataset loader, stereo pairing, calibration setup, baseline extraction, **`imu0/data.csv` loader, multi-line `T_BS` YAML folding, `cam0FromImuExtrinsic` SE3 publishing with SVD re-orthonormalization** +- `euroc_dataset.h` (`~100`): EuRoC dataset API, **`allImu()` / `getImuBetween()` / `hasCam0FromImuExtrinsic()`** - `euroc_pinhole_calibration.cc` (`188`): EuRoC JSON calibration parser - `euroc_pinhole_calibration.h` (`34`): EuRoC calibration structs/API - `map_io.cc` (`269`): map save/load @@ -354,8 +436,10 @@ These are outside the directory inventory requested below but matter immediately #### `tests/` +- 18 files total, `ctest -j4` reports 77 / 77 PASS - `test_camera.cc` (`59`): camera projection/unprojection tests -- `test_euroc_dataset.cc` (`124`): EuRoC dataset and stereo-calibration tests +- `test_euroc_dataset.cc` (`~260`): EuRoC dataset and stereo-calibration tests, **`cam0 T_BS` single-line and multi-line YAML, IMU CSV**, PID + nanosecond-unique temp paths +- `test_imu_preintegrator.cc` (`~90`): zero-motion identity, constant-accel `deltaV`/`deltaP`, `predict` with gravity, bias subtraction, reset - `test_frame.cc` (`90`): frame depth and backprojection tests - `test_initializer.cc` (`29`): initializer smoke/regression tests - `test_keyframe.cc` (`74`): covisibility ranking and connection tests @@ -363,7 +447,8 @@ These are outside the directory inventory requested below but matter immediately - `test_loop_closing.cc` (`83`): loop weighting and stale-edge decay tests - `test_map.cc` (`96`): map add/remove/concurrency tests - `test_metric_depth_estimator.cc` (`40`): metric-depth tensor-shape/model tests -- `test_optimizer.cc` (`149`): BA, pose graph, depth prior, gravity prior tests +- `test_optimizer.cc` (`~345`): BA, pose graph, depth prior, gravity prior tests, **VelocityDelta prior, 9-DoF preintegration residual (match, gravity, accel-bias first-order, rotation match, rotation mismatch), bias anchor, bias random-walk** +- `test_visual_inertial_initializer.cc` (`~255`): scale + gravity recovery on synthetic EuRoC-like scene, capped closed-form gyro-bias recovery, metric-scale mode, missing-span rejection - `test_reference_keyframe_policy.cc` (`88`): heuristic/score/pipeline policy tests - `test_stereo_depth_estimator.cc` (`87`): stereo-depth correctness tests - `test_synthetic_scene.h` (`79`): shared synthetic-scene helper for tests @@ -625,26 +710,48 @@ These are the hard-coded runtime knobs that matter most. They are the first plac | `src/depth/stereo_depth_estimator.cc:98-109` | StereoSGBM params | block `5`, uniqueness `10`, speckle `50`, range `2`, `disp12=1` | current stereo matcher setup | | `src/depth/metric_depth_estimator.cc:84` | ONNX intra-op threads | `4` | metric-depth inference thread count | +### 7.8 VIO Env Knobs (all optional — defaults preserve the Stage 0c empirical state) + +| Env variable | Default | Meaning | +| --- | ---: | --- | +| `SVSLAM_VIO_VELOCITY_IMU_ALPHA` | `0.3` | Blend weight for the IMU prediction vs the post-track visual pose delta inside `reconcileVelocityWithVisual`. 0 = pure visual, 1 = pure IMU open-loop | +| `SVSLAM_VIO_ENABLE_VISUAL_VELOCITY` | unset | Opt-in: populate `Frame::velocity_` on non-IMU runs (TUM) so BA's velocity prior acts as a motion-smoothness regularizer | +| `SVSLAM_BA_VELOCITY_PRIOR_SIGMA_M` | `0.3` (m) | Position-side sigma for the preintegration + loose-delta residuals. MH_01 sweep says this is optimal; `<=0` disables the velocity-related residuals entirely | +| `SVSLAM_BA_VELOCITY_PRIOR_VEL_SIGMA` | `0.3` (m/s) | Velocity-side sigma for the preintegration residual | +| `SVSLAM_BA_PREINT_ROT_SIGMA_RAD` | `0.05` (≈ 2.9°) | Rotation residual sigma per KF-gap. `<=0` zeros the rotation weight, restoring 6-DoF behavior | +| `SVSLAM_BA_BIAS_ACCEL_ANCHOR_SIGMA` | `0.5` (m/s²) | Accel-bias anchor sigma | +| `SVSLAM_BA_BIAS_GYRO_ANCHOR_SIGMA` | `0.1` (rad/s) | Gyro-bias anchor sigma | +| `SVSLAM_BA_BIAS_ACCEL_RW_SIGMA` | `0.05` (m/s² per pair) | Accel-bias random-walk sigma between consecutive KFs | +| `SVSLAM_BA_BIAS_GYRO_RW_SIGMA` | `0.005` (rad/s per pair) | Gyro-bias random-walk sigma | +| `SVSLAM_BA_GRAVITY_PRIOR_WEIGHT` | `2.0` | Per-KF gravity prior weight. `<=0` disables; useful on aggressive-motion sequences where the ±50 ms accel window is dominated by motion rather than gravity | +| `SVSLAM_VIO_MIN_INIT_KEYFRAMES` | `15` | Minimum KFs before `tryVisualInertialInit` attempts the linear solve | +| `SVSLAM_VIO_GATE_PREINT` | unset | Opt-in: restore the original "gate preintegration until VI init succeeds" behavior. Default is preint always on — safer on sequences where VI init never converges (e.g. MH_01) | +| `SVSLAM_VIO_FORCE_PREINT_BA` | unset | Legacy inverse of `SVSLAM_VIO_GATE_PREINT`, kept for backwards-compatibility with pre-2026-04-20 benchmark reruns | + +Tuning philosophy: the defaults above were arrived at on EuRoC MH_01 with a four-point σ sweep; they leave the residuals noticeably *loose* so visual BA wins ties. Do not tighten `SVSLAM_BA_VELOCITY_PRIOR_SIGMA_M` below `0.2` without fresh evidence — MH_01 regressed 2× at σ=0.1. + --- ## 8. Known Issues -Ranked by severity: +Ranked by severity for **2026-04-21**: 1. **`room_mono` remains far behind `stella_vslam` and still lacks early loop detections.** - - Current gate: `0.197374 m` + - Current gate: `0.176506 m` (improved from `0.197374 m` after the pre-VIO work, 2026-04-16) - `stella_vslam`: `0.02743546 m` - Published loop-enabled comparison recorded `0/0/0` loop detections inside the first 250 frames. + - **plan.md §10.4's ordered experiment #1 (ORB 2000 → 3000) was re-tested on 2026-04-20 and regresses both mono room gates — see §8.12.** -2. **`xyz_mono_head_repro` is passing again, but the margin is thin.** - - Current value: `0.028136 m` - - Ceiling: `0.030000 m` - - Any further mono changes should rerun `xyz_mono_head_repro` immediately. +2. **`xyz_mono_*` gates pass with thin margins.** + - `xyz_mono_head_repro`: `0.028136 m` / ceiling `0.030000 m` (7 % slack) + - `xyz_mono_accel_head_repro`: `0.024931 m` / ceiling `0.027000 m` (7 % slack) + - `room_mono_accel_head_repro`: `0.164001 m` / ceiling `0.170000 m` (4 % slack — thinnest) + - Any further mono front-end change must run these three gates before and after. 3. **`room_depth` is still materially behind `stella_vslam`.** - - Best retained number: `0.0695 m` - Current strict repro gate: `0.079914 m` - - `stella_vslam`: `0.02110508 m` + - `stella_vslam`: `0.02110508 m` (≈ 3.8× gap) + - ORB 3000 experiment incidentally improved this to `0.068392 m` but was reverted because it broke room_mono — suggests there is room to move if mono damage can be avoided. 4. **600-frame loop documentation:** see **`eval/room_depth_600frame_report.md`** (2026-04-15). The older `~0.618 m` median in `eval/stella_comparison_results.md` remains historical context only. @@ -652,21 +759,44 @@ Ranked by severity: - Code mostly snapshots immediately, but the API is still easy to misuse - See `src/core/map.cc:27-33` -6. **Real EuRoC verification is still missing.** - - The code path was verified only on the synthetic fallback dataset - - Loader semantics and stereo baseline path are good, but real-sequence performance is unverified - -7. **Metric depth works on `xyz` but is poor on `room` at 250 frames.** +6. **Metric depth works on `xyz` but is poor on `room` at 250 frames.** - `room 250`: `0.38429766 m` - Sensor depth on the same scenario is much better -8. **The ROS2 node is functional but not feature-parity with the CLI.** - - It currently wraps `Tracking` + `LocalMapping` +7. **The ROS2 node is functional but not feature-parity with the CLI.** - No loop-closing thread is started in `ros2/src/slam_node.cc` - -9. **The README "6k lines" claim is now stale.** - - Current measured app+core count is `9715` - - This is a documentation issue, not an algorithmic blocker + - **No VIO hookup either** — `Tracking::imu_buffer_` / `Tracking::setImuToCameraExtrinsic` are unused in the ROS2 path + +8. **The README "6k lines" claim is now stale.** + - Current measured app+core count is `12 090` + - Documentation issue, not an algorithmic blocker + +9. **VI init rejects noisy-mono sequences without falling through to ORB-SLAM3-style MAP refinement.** + - MH_01 and MH_03 both have rot_rms ≈ 0.13 rad during the first 15 KFs, above the 0.08 rad threshold + - V2_01 rejects with "scale outside tolerance" (scale ≈ 0.007 — degenerate visual window) + - Only V1_01 accepts; gain is modest (scale correction ≈ 2 %) + - Follow-up: §16.5 + +10. **Gyro-bias first-order Jacobian inside the BA rotation residual was tried and reverted (2026-04-20).** + - Adds a free parameter that absorbs visual rotation noise when biases are not pre-calibrated + - Regressed MH_01 from `1.41 m` to `2.68–3.56 m` at any gyro-anchor σ in `[0.01, 0.1]` + - **Do not re-enable without a VI-init stage that delivers gyro bias to O(0.01 rad/s) first** + +11. **MH_03_medium is the open VIO regression.** + - Visual-only `2.48 m` → +VIO `2.91 m` (+17 %) + - Disabling preintegration residual (`SVSLAM_BA_VELOCITY_PRIOR_SIGMA_M=0`): `3.04 m` — still worse than visual-only + - Disabling gravity prior (`SVSLAM_BA_GRAVITY_PRIOR_WEIGHT=0`): `2.90 m` — essentially no change + - Hypothesis: MH_03's aggressive motion makes every IMU-path contribution noisy. The 126 `Lost` events vs 82 in the visual-only run suggest the first `predictVelocityFromImu` result or the per-KF gravity alignment from a ±50 ms aggressive-motion window is destabilizing Tracking. Not yet root-caused; see §16.4 for the next bisection to try. + +12. **`plan.md` §10 ordered experiments are partially stale.** + - §10.4 #1 ORB 2000 → 3000 regresses both mono room gates (`0.177 m` → `0.227 m`; `0.164 m` → `0.236 m`, above the `0.170 m` ceiling) despite plan.md predicting "strongest immediate increase in mono match density" + - §10.4 #4 / §10.2 #1 (initializer H/F ratio `0.50 → 0.60`) is already in the code + - Remaining items should be probed one-by-one on the narrowest relevant gate. See §12 for the recommended workflow + +13. **EuRoC stereo-only depth is degraded (pre-existing, not a VIO regression).** + - MH_01 `--stereo` no-IMU ATE ≈ `2.77 m` vs `3.44 m` visual mono (worse than mono on the long-distance flight) + - Pinhole stereo rectification is not handling EuRoC's fisheye-ish distortion well + - Unblocks the stereo tracking mode (Feature Matrix "Partial" row). Orthogonal to VIO work. --- @@ -694,7 +824,24 @@ Ranked by severity: | Status | What is done | What remains | | --- | --- | --- | -| Partial | EuRoC loader, EuRoC stereo depth, ROS2 node, metric-depth CLI | real EuRoC validation, stronger ROS2 parity, true stereo/tracking evolution, IMU tight coupling if ever prioritized | +| Partial | EuRoC loader, EuRoC stereo depth, ROS2 node, metric-depth CLI, VIO Stage 0b (IMU preintegration scaffolding), VIO Stage 0c (BA preintegration residual + bias BA params + VI init), empirical MH_01/V1_01 ATE validation | VIO Stage 0c.f (ORB-SLAM3-style MAP refinement, larger window, gyro-bias Jacobian once biases converge), stronger ROS2 parity, true stereo/tracking evolution | + +#### VIO status (2026-04-20) + +Commits `bebc7d5..bd7b691` landed the loosely-coupled VI pipeline. Validated on EuRoC mono + `--accel`: + +| Dataset | Visual only | +VIO (Stage 0c.d+e) | +| --- | ---: | ---: | +| MH_01_easy | 3.44 m | **1.41 m** (−59%) | +| V1_01_easy | 1.25 m | **1.31 m** | + +- VI Init accepts V1_01 (rot_rms 0.05 rad < 0.08 threshold) and rejects MH_01 (rot_rms 0.13 rad). MH_01 benefits from the 9-DoF BA residual alone; V1_01 picks up additional gain from scale + gravity refinement. +- Loose sigma defaults: `SVSLAM_BA_VELOCITY_PRIOR_SIGMA_M=0.3` m, `SVSLAM_BA_PREINT_ROT_SIGMA_RAD=0.05` rad. MH_01 sweep shows σ=0.3 is optimal; tighter pulls poses too hard, looser discards IMU info. +- Tried but reverted: gyro-bias first-order Jacobian inside the BA rotation residual. It gives BA a free parameter that absorbs visual rotation noise when biases are uncalibrated, regressing MH_01 by 2–3×. Re-visit only after a reliable VI init calibrates gyro bias. + +Known limitations: +- Mono + `--stereo` on EuRoC yields ATE ~2.8 m even visual-only — the pinhole-rectified stereo pipeline is not handling EuRoC's fisheye-ish distortion well. Separate from the VIO work. +- Gyro bias estimate from the linear VI init is unreliable on short EuRoC windows (O(0.05) rad/s vs O(0.004) ground truth); currently hard-capped at 0.05 rad/s and BA takes over via random-walk + anchor priors. ### Phase E: Community / Release Hygiene @@ -858,9 +1005,9 @@ Ordered experiments: - Risk: - can increase false positives, so do this only after checking logs for candidate starvation -### 10.4 `room_mono` gap: about `7.2x` +### 10.4 `room_mono` gap: about `6.5x` -- SimpleVisualSLAM current gate: `0.197374 m` +- SimpleVisualSLAM current gate: `0.176506 m` (improved from `0.197374 m` after the pre-VIO mono-room work) - `stella_vslam`: `0.02743546 m` - Published loop-enabled comparison saw **no loop detections inside the first 250 frames** @@ -872,12 +1019,11 @@ Most likely causes: Ordered experiments: -1. `apps/run_mono.cc:311` - - Change: - - `cv::ORB::create(2000)` -> `3000` - - Expected effect: - - strongest immediate increase in mono match density - - helps both relocalization and loop detection +1. ~~`apps/run_mono.cc`: `cv::ORB::create(2000)` → `3000`~~ **TESTED 2026-04-20 — REGRESSED, reverted.** Predicted "strongest immediate increase in mono match density" but: + - `room_mono_head_repro`: `0.177 m` → `0.227 m` (worse) + - `room_mono_accel_head_repro`: `0.164 m` → `0.236 m` (above `0.170 m` ceiling) + - `room_depth_head_repro`: `0.080 m` → `0.068 m` (actually improved) + - Mono is more sensitive to feature count at the 250-frame boundary than plan.md anticipated — more keypoints = more outlier BA candidates when the map is still thin. Do not re-try without pairing it with a depth-detecting gate bypass. 2. `src/tracking/tracking.cc:775-824` - Change for mono only: @@ -963,36 +1109,59 @@ If threshold changes plateau, these are the next non-trivial algorithmic moves: ## 11. Priority -Work on these in order: +Ordered for **2026-04-21 → next session**. Do the practical items first, then deep-dive: + +1. **Land PR #3.** + - Branch: `vio-integration`, head `8e6f4e7`, not yet merged. 77/77 tests, 7/7 gates, EuRoC cross-seq validated. + - After merge, bump local `master`, rerun `ctest` + `scripts/check_regression_gate.py --all-gates` as a smoke check, delete the local branch. + +2. **Diagnose MH_03 regression (§8.11 / §16.4).** + - 5-way IMU-path bisection: (a) accel_buffer_ population only, (b) gravity alignment only, (c) gravity prior only, (d) `predictVelocityFromImu` only, (e) `reconcileVelocityWithVisual` only. Binary-search which sub-path introduces the +0.4 m vs visual-only. + - Success criterion: MH_03 mean ATE at or below visual-only `2.48 m` with `--accel` on. -1. **Close the `room_mono` gap enough to get below `0.20 m`, then below `0.15 m`, while keeping `xyz_mono_head_repro <= 0.030000 m`.** - - Start with selective mono reference refresh / keyframe rules, not broad global threshold changes. +3. **Commit a proper `eval/` artifact for the EuRoC cross-seq sweep.** + - Mirror `eval/room_depth_600frame_report.md`. Include sequence, frames, mean/median/max ATE, the exact `run_mono` command, the `evo_ape` command, and the commit hash. + - Same-day follow-up: a machine-readable `eval/euroc_vio_sweep.json` that `scripts/build_leaderboard.py` can ingest. -2. ~~**Strengthen `room_depth` pose graph.**~~ **Done (2026-04-15):** sequential edges between time-adjacent keyframes in `Optimizer::poseGraphOptimization` (`src/backend/optimizer.cc`); `room_depth_head_repro` measured `~0.080 m` on `build_codex`. +4. **Probe `plan.md` §10 experiments one at a time.** + - §10.1 (xyz_depth) is the smallest gap and the lowest risk — start there. + - Rule: narrowest gate first, all-gates second, revert on any ceiling breach. + - Update the §10 table entry (✅ kept / ❌ reverted + measured numbers) inside the same commit. -3. ~~**Refresh `eval/stella_comparison_results.md` (fair head-250).**~~ **Done (2026-04-15):** top table + `eval/stella_comparison.json`; deeper sections in the markdown remain historical. +5. **Phase D deep-dives.** Choose one; do not stack. + - **ORB-SLAM3-style MAP VI init (§16.5):** non-linear joint optimization over {scale, gravity, velocities, accel_bias, gyro_bias} on the first ~30 KFs. Goal: make MH_01 accept VI init with rot_rms ≤ 0.08 rad. + - **EuRoC stereo rectification fix (§8.13):** proper fisheye / radtan support so `--stereo` on EuRoC isn't a regression vs mono. + - **ROS2 VIO hookup:** wire `Tracking::imu_buffer_` / `setImuToCameraExtrinsic` through `ros2/src/slam_node.cc`. -4. ~~**Write a fresh current-HEAD 600-frame room-depth report into `eval/`.**~~ **Done:** `eval/room_depth_600frame_report.md`. +Previously-priority items now complete: -5. **Replace the synthetic EuRoC fallback with a real-sequence validation once dataset access is solved.** +- ~~`xyz_mono` recovery~~ (done, `0.028 m` gate passing) +- ~~`room_depth` pose graph~~ (done 2026-04-15) +- ~~`eval/stella_comparison_results.md` fair head-250 refresh~~ (done) +- ~~600-frame loop report~~ (`eval/room_depth_600frame_report.md`) +- ~~Real EuRoC verification~~ (done 2026-04-19 / 2026-04-20 via Wayback; 5 sequences on disk, see §2.7–§2.8) +- ~~VIO Stage 0b / 0c~~ (landed on `master`; PR #3 pending) --- ## 12. AI Agent Instructions 1. Read in this order: - - this file + - **this file** (especially §2.1, §2.4, §2.7, §2.8, §8, §11, §16) - `apps/run_mono.cc` - - `src/tracking/tracking.cc` + - `src/tracking/tracking.cc` + `tracking.h` - `src/tracking/initializer.cc` + - `src/tracking/visual_inertial_initializer.{h,cc}` (if touching the VIO path) - `src/backend/local_mapping.cc` - - `src/backend/optimizer.cc` + - `src/backend/optimizer.{h,cc}` (the residual definitions live in the header) + - `src/sensors/imu_preintegrator.h` + `imu_preintegration_span.h` - `src/loop_closing/loop_closing.cc` 2. Use the narrowest benchmark that can validate your change: - - mono front-end changes: `xyz_mono_head_repro`, then `room_mono_head_repro` - - pose-graph / loop changes: `room_depth_head_repro`, then loop-enabled comparison preset - - metric-depth changes: rerun the metric-depth scripts on `xyz` before touching `room` + - **mono front-end**: `xyz_mono_head_repro` *first* (thin margin), then `room_mono_head_repro`, then `room_mono_accel_head_repro` (thinnest margin — 4 %). Only after all three pass, run `--all-gates`. + - **pose-graph / loop**: `room_depth_head_repro`, then the loop-enabled comparison preset + - **metric-depth**: rerun the metric-depth scripts on `xyz` before touching `room` + - **VIO**: EuRoC mono `--accel` on MH_01 *first* (best-behaved, biggest gain), then cross-check on V1_01 (VI init accepts) and MH_03 (open regression; at least should not regress more). Visual-only baseline on the same sequence is a pre-requisite: run it too. 3. For threaded changes, test both modes: - `--repro-eval` @@ -1003,11 +1172,22 @@ Work on these in order: - do not propagate retained-note numbers into public docs without a fresh rerun 5. Do not silently change protocol: - - keep `evo_ape` flags aligned with `eval/regression_baselines.json` + - keep `evo_ape` flags aligned with `eval/regression_baselines.json` for TUM gates (`--align --correct_scale --t_max_diff 0.05`) + - use the same flags for EuRoC mono (Sim3 is mandatory — mono scale is unobservable) - do not redefine what a regression gate means without updating the harness/docs together 6. Do not commit unless explicitly asked. - If a human later asks for a commit, keep one logical change per commit and include the exact validation command(s). + - **Never add AI-generated markers** ("Generated with ...", "Co-Authored-By: Claude", etc.) to commit messages or PR descriptions — this project forbids them. + +7. When probing `plan.md` §10 experiments, trust the *direction* but not the *numbers*: + - The §10 lists were written against an earlier codebase state. Predicted gains have flipped sign at least once (ORB 3000 — see §8.12). + - Rule: narrowest gate first, all-gates second, revert on any ceiling breach, update the §10 table entry in the same commit with the measured delta and the date. + +8. When touching VIO: + - Default sigmas (§7.8) were tuned empirically on EuRoC. Do not tighten `SVSLAM_BA_VELOCITY_PRIOR_SIGMA_M` below `0.2` without fresh evidence on MH_01. + - The gyro-bias first-order Jacobian inside the BA rotation residual was tried and reverted (§8.10). Do not re-enable without a working VI init first. + - `plan.md` §16 is the VIO-specific handoff; read it before making any change that touches `ImuPreintegrationSpan`, `VelocityPreintegrationError`, or the `VisualInertialInitializer`. --- @@ -1149,7 +1329,9 @@ Best achieved: **0.01104** — still 1.24x behind stella (0.00889). The gap is l - Don't claim stella_vslam is beaten until reproduced 3x with identical protocol - Don't modify regression gate semantics without updating baselines -## 15. Mono Room Handoff (2026-04-15) +## 15. Mono Room Handoff (2026-04-15) — historical + +> **Historical note (2026-04-21):** this section was written when `room_mono_head_repro` was the single active frontier and VIO work had not started. The §15.1 safe-state number (`0.197374 m`) has since moved to `0.176506 m` on the current master. The guidance inside this section (favor ranking/selection over threshold widening; frame-199 diagnostic; trace-only diffs before behavior changes) is still valid for any mono-front-end change. Treat everything below as design context, not a current TODO — §16 supersedes it for the VIO frontier, and §11 / §10.4 carry the current mono TODO. This section is the immediate handoff for the next agent. It is intentionally more concrete than the older battle log above because the active frontier is now very narrow: `room_mono_head_repro` is the only materially weak gate left in the head-250 harness, and repeated broad threshold sweeps have already been ruled out. @@ -1388,7 +1570,130 @@ sed -n '3928,3955p' log/room_mono_trace_next.log rg -n "BootstrapStats|Relocalize: Candidate KF|Relocalize: Matched with KF" log/room_mono_trace_next.log ``` -## 16. Non-Goals +## 16. VIO Handoff (2026-04-21) + +This section supersedes §15 for any change that touches the IMU path. §15 remains the right handoff for mono-only front-end work. + +### 16.1 Current Safe State + +Master: `46a726d`. PR #3 (branch `vio-integration`, head `8e6f4e7`) adds a flaky-test fix and a gravity-prior env knob. + +Build + tests: + +```bash +cmake -S . -B build -G Ninja -DBUILD_TESTS=ON +cmake --build build -j$(nproc) --target svslam_core svslam_tests run_mono +ctest --test-dir build --output-on-failure # 77 / 77 PASS +python3 scripts/check_regression_gate.py --build $PWD/build --data-tum $PWD/data/tum --all-gates --quiet +# → 7 / 7 PASS, bitwise-identical trajectories +``` + +EuRoC mono `--accel` baselines (reproduce before changing anything): + +```bash +./build/run_mono --euroc datasets/euroc/MH_01_easy --accel --reference-policy heuristic \ + --skip-frames 0 --no-viz --repro-eval +evo_ape tum datasets/euroc/MH_01_easy/gt_tum.txt build/trajectory.txt \ + --align --correct_scale --t_max_diff 0.05 --no_warnings +# → mean ~1.41 m +``` + +Matching visual-only baseline (same command without `--accel`) → `3.44 m`. + +### 16.2 Pipeline Diagram + +``` +Frame N (mono image) ───┐ + │ + ▼ + Tracking::addFrame + │ + ▼ + Tracking::track + ├─ motion-model pose predict + ├─ stationary detection (accel_buffer_) + ├─ predictVelocityFromImu ──► Frame::velocity_ (IMU body, world) + ├─ trackReferenceKeyframe (reprojection / PnP) + ├─ trackLocalMap + ├─ reconcileVelocityWithVisual ──► blended Frame::velocity_ + └─ if needNewKeyframe: + Keyframe::Keyframe(Frame) ─► copies velocity_, biases + setKeyframeGravity (gravity in camera frame) + populateKeyframeImuSpan ─► ImuPreintegrationSpan (delta_R/v/p, dt, T_cam_imu, from_kf_id) + tryVisualInertialInit (if enough KFs and vi_init_done_ == false) + └─ VisualInertialInitializer::initialize + ├─ Stage 1: closed-form gyro bias (capped at 0.05 rad/s) + └─ Stage 2: LSQ for {scale, gravity, velocities} + └─ on success: rescale map, rotate world to gravity-Z, + applyGyroBiasCorrectionToSpans (first-order Forster), + Optimizer::setPreintegrationResidualEnabled(true) + +LocalMapping::optimization ─► Optimizer::bundleAdjustment + ├─ ReprojectionError (per landmark observation) + ├─ DepthPriorError (if depth available) + ├─ GravityPriorError (per KF with has_gravity_, env-tunable weight) + ├─ VelocityPreintegrationError (9-DoF, when prev_imu_span_ valid) + ├─ VelocityDeltaPriorError (loose fallback) + ├─ BiasAnchorError * 2 (accel + gyro anchors) + └─ BiasRandomWalkError * 2 (between consecutive KFs) +``` + +### 16.3 Key Invariants + +- `Frame::velocity_` is world-frame velocity of the **IMU body**, not the camera. The lever-arm term `t_bc` is baked into `VelocityPreintegrationError` so BA reconciles properly. +- `ImuPreintegrationSpan::T_cam_imu` is captured at span-creation time, not looked up at BA time. If the EuRoC extrinsic ever becomes dynamic, revisit this. +- Rotation alignment matters: the `initializeWithDepth` / `initialize` paths now rotate the gravity estimate from the IMU frame into the camera frame before building `T_align`. This was a silent bug for TUM (IMU ≈ camera) but catastrophic on EuRoC before the fix landed in `bebc7d5`. +- Default: preintegration residual is **always on** when `has_velocity_` is set. VI init's role is to refine scale / gravity / velocities + apply the first-order gyro correction to spans, *not* to gate the residual. The old gated behavior is available via `SVSLAM_VIO_GATE_PREINT=1` but has been shown to regress MH_01 catastrophically when VI init rejects (1.41 → 3.46 m). + +### 16.4 Open Regression — MH_03_medium + +Visual-only `2.48 m`, +VIO `2.91 m` (+17 %). The `--accel` path clearly hurts, but none of the individual components fully explains it: + +| Config | Mean ATE | +| --- | ---: | +| Visual-only | `2.48 m` | +| +IMU (full default config) | `2.91 m` | +| +IMU, `SVSLAM_BA_VELOCITY_PRIOR_SIGMA_M=0` (preint off) | `3.04 m` | +| +IMU, `SVSLAM_BA_GRAVITY_PRIOR_WEIGHT=0` (gravity off) | `2.90 m` | + +Lost-tracking events: 82 visual-only, 103–126 with `--accel`. Something in the IMU path is destabilizing Tracking itself, not just adding bad BA residuals. + +Next bisection to try (one at a time, commit env knobs as needed): + +1. Run with `--accel` but short-circuit `predictVelocityFromImu` to a no-op. If MH_03 recovers, the initial velocity estimate is the culprit. +2. Short-circuit `setKeyframeGravity` (skip per-KF gravity direction). Eliminates noisy per-KF gravity propagation without touching the init-time alignment. +3. Short-circuit `reconcileVelocityWithVisual`'s IMU path. Isolates the blended-velocity path from the raw-IMU path. +4. As a last step, keep only `accel_buffer_` populated (so gravity init fires once, but no per-frame IMU work). If MH_03 still regresses, the init-time `T_align` on MH_03's accel window is miscomputed. + +Likely culprit on prior: short-circuit (1) or the init-time gravity alignment (4). MH_03 has aggressive flight, so a ±50 ms accel mean can drift noticeably from true gravity. + +### 16.5 Stage 0c.f — Follow-ups (not in this PR) + +Three candidate directions, ranked by expected payoff per hour: + +1. **ORB-SLAM3-style MAP-based VI init refinement.** After the current linear two-stage solve, run a Ceres problem over the first ~30 KFs with fixed visual poses and free {scale, gravity, per-KF velocities, accel_bias, gyro_bias}. Preintegration residuals + soft bias priors. Should make MH_01 and MH_03 accept VI init by reducing the rot_rms below `0.08 rad` — the current linear gyro-bias solve is too short-window-dependent. +2. **Gyro-bias first-order Jacobian in the BA rotation residual** (re-tried after #1). The current reverted attempt regressed MH_01 2–3× because BA absorbed visual rotation noise into `bg`. Once the MAP init above delivers `bg` ≈ O(0.01 rad/s), the Jacobian becomes a refinement, not an over-fit. Gate behind `SVSLAM_BA_PREINT_ENABLE_GYRO_JACOBIAN=1` to avoid default surprises. +3. **Longer VI-init window + source ranking.** Today `SVSLAM_VIO_MIN_INIT_KEYFRAMES=15`. Accepting at 25–30 should help V1_01-class windows where the first few mono rotations are noisier. Must be paired with a `max_init_attempts` cap so we stop retrying after the window stabilizes. + +### 16.6 Reference Files For Any VIO Change + +Minimum set to read and understand before changing anything VIO-side: + +- `src/sensors/imu.h` (ImuEntry value type) +- `src/sensors/imu_preintegrator.h` (math) +- `src/sensors/imu_preintegration_span.h` (per-KF-pair state) +- `src/core/frame.h` and `core/keyframe.h` (where velocity / biases / spans live) +- `src/tracking/tracking.h` + the VIO-named methods in `tracking.cc` (`predictVelocityFromImu`, `reconcileVelocityWithVisual`, `populateKeyframeImuSpan`, `tryVisualInertialInit`, plus the `gravity_aligned_` init paths) +- `src/tracking/visual_inertial_initializer.{h,cc}` (linear 2-stage solve) +- `src/backend/optimizer.h` — **all VIO residuals are defined here**: `VelocityPreintegrationError`, `VelocityDeltaPriorError`, `BiasAnchorError`, `BiasRandomWalkError` +- `src/backend/optimizer.cc` — the `bundleAdjustment` function plumbs them; search for "preintegration" in that file +- `apps/run_mono.cc` — EuRoC setup path, especially `setImuToCameraExtrinsic` and the preint-gate opt-in + +Minimum reproducing commands are in §16.1. Once you have new numbers, update §2.8, §8, §11 in the same commit. + +--- + +## 17. Non-Goals Do **not** spend time on these unless a human explicitly reprioritizes them: diff --git a/src/backend/optimizer.cc b/src/backend/optimizer.cc index c326fef..8666ea1 100644 --- a/src/backend/optimizer.cc +++ b/src/backend/optimizer.cc @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -14,6 +15,24 @@ namespace svslam { namespace { +// Gate shared with Tracking. Default true matches the Stage 0c behaviour +// (preintegration residual always on) so datasets that do VI init still +// see identical results, while Tracking can opt out for the pre-init +// window by calling setPreintegrationResidualEnabled(false). +std::atomic g_preintegration_residual_enabled{true}; + +} // namespace + +void Optimizer::setPreintegrationResidualEnabled(bool on) { + g_preintegration_residual_enabled.store(on, std::memory_order_relaxed); +} + +bool Optimizer::preintegrationResidualEnabled() { + return g_preintegration_residual_enabled.load(std::memory_order_relaxed); +} + +namespace { + // Default 1 for repeatable BA; use SVSLAM_CERES_NUM_THREADS=N (integer >= 1) for faster solves. int ceres_num_threads_from_env() { const char* env = std::getenv("SVSLAM_CERES_NUM_THREADS"); @@ -497,28 +516,268 @@ void Optimizer::bundleAdjustment(const std::vector& keyframes, std::cout << "BA: Added " << depth_residual_count << " depth prior residuals" << std::endl; } - // Add gravity prior residuals for keyframes with accelerometer data + // Add gravity prior residuals for keyframes with accelerometer data. + // Weight tunable via SVSLAM_BA_GRAVITY_PRIOR_WEIGHT (default 2.0); <=0 + // disables the prior entirely, useful on aggressive-motion sequences + // where the ±50 ms accel window is dominated by motion rather than + // gravity (e.g. EuRoC MH_03_medium). + double gravity_weight = 2.0; + if (const char* env = std::getenv("SVSLAM_BA_GRAVITY_PRIOR_WEIGHT")) { + char* end = nullptr; + const double parsed = std::strtod(env, &end); + if (end != env && std::isfinite(parsed)) { + gravity_weight = parsed; + } + } int gravity_residual_count = 0; - for (auto& kf : keyframes) { - if (!kf || !kf->has_gravity_) continue; - if (pose_params.find(kf->id_) == pose_params.end()) continue; - if (problem.IsParameterBlockConstant(pose_params[kf->id_])) continue; - - // Gravity is only approximately camera-aligned on TUM, so keep this prior soft. - double gravity_weight = 2.0; - ceres::CostFunction* gravity_cost = GravityPriorError::Create( - kf->gravity_in_camera_.x(), kf->gravity_in_camera_.y(), kf->gravity_in_camera_.z(), - gravity_weight); - - problem.AddResidualBlock(gravity_cost, new ceres::HuberLoss(0.3), - pose_params[kf->id_]); - gravity_residual_count++; + if (gravity_weight > 0.0) { + for (auto& kf : keyframes) { + if (!kf || !kf->has_gravity_) continue; + if (pose_params.find(kf->id_) == pose_params.end()) continue; + if (problem.IsParameterBlockConstant(pose_params[kf->id_])) continue; + + ceres::CostFunction* gravity_cost = GravityPriorError::Create( + kf->gravity_in_camera_.x(), kf->gravity_in_camera_.y(), kf->gravity_in_camera_.z(), + gravity_weight); + + problem.AddResidualBlock(gravity_cost, new ceres::HuberLoss(0.3), + pose_params[kf->id_]); + gravity_residual_count++; + } } if (gravity_residual_count > 0) { std::cout << "BA: Added " << gravity_residual_count << " gravity prior residuals" << std::endl; } + // Velocity priors between consecutive (in time) local keyframes. Two + // flavors: (1) a Forster preintegration residual whenever the successor + // KF carries an ImuPreintegrationSpan from its predecessor (Stage 0c.c); + // (2) the legacy loose "position delta ≈ v*dt" prior for any remaining + // pairs where a span is missing. Sigma tuning applies to both flavors. + double velocity_sigma_m = 0.3; + if (const char* env = std::getenv("SVSLAM_BA_VELOCITY_PRIOR_SIGMA_M")) { + char* end = nullptr; + const double parsed = std::strtod(env, &end); + if (end != env && std::isfinite(parsed)) { + velocity_sigma_m = parsed; + } + } + + // Separate velocity-residual sigma. Meaningful only with the preintegration + // residual; the loose fallback ignores it. Default matches position sigma. + double velocity_vel_sigma = 0.3; + if (const char* env = std::getenv("SVSLAM_BA_VELOCITY_PRIOR_VEL_SIGMA")) { + char* end = nullptr; + const double parsed = std::strtod(env, &end); + if (end != env && std::isfinite(parsed) && parsed > 0.0) { + velocity_vel_sigma = parsed; + } + } + + std::map velocity_params; // owned, freed below + std::map accel_bias_params; + std::map gyro_bias_params; + int velocity_residual_count = 0; + int preintegration_residual_count = 0; + int bias_random_walk_residual_count = 0; + int bias_anchor_residual_count = 0; + + // Bias priors are loose by default — BA should absorb IMU scale errors + // slowly, not fight them. Env-tunable so experiments can dial them in. + double bias_accel_anchor_sigma = 0.5; // m/s^2 + double bias_gyro_anchor_sigma = 0.1; // rad/s + double bias_accel_rw_sigma = 0.05; // m/s^2 per KF-gap + double bias_gyro_rw_sigma = 0.005; // rad/s per KF-gap + auto read_pos = [](const char* name, double& sink) { + if (const char* env = std::getenv(name)) { + char* end = nullptr; + const double parsed = std::strtod(env, &end); + if (end != env && std::isfinite(parsed) && parsed > 0.0) { + sink = parsed; + } + } + }; + read_pos("SVSLAM_BA_BIAS_ACCEL_ANCHOR_SIGMA", bias_accel_anchor_sigma); + read_pos("SVSLAM_BA_BIAS_GYRO_ANCHOR_SIGMA", bias_gyro_anchor_sigma); + read_pos("SVSLAM_BA_BIAS_ACCEL_RW_SIGMA", bias_accel_rw_sigma); + read_pos("SVSLAM_BA_BIAS_GYRO_RW_SIGMA", bias_gyro_rw_sigma); + if (velocity_sigma_m > 0.0 && keyframes.size() >= 2) { + std::vector ordered_keyframes; + ordered_keyframes.reserve(keyframes.size()); + for (const auto& kf : keyframes) { + if (kf) ordered_keyframes.push_back(kf); + } + std::sort(ordered_keyframes.begin(), ordered_keyframes.end(), + [](const Keyframe::Ptr& a, const Keyframe::Ptr& b) { + return a->id_ < b->id_; + }); + + // Register a velocity parameter block for every local KF that has a + // usable estimate. The block is freshly allocated so BA solves write + // into scratch instead of mutating kf->velocity_ mid-solve. + auto addVelocityBlock = [&](const Keyframe::Ptr& kf) -> double* { + if (!kf || !kf->has_velocity_ || !kf->velocity_.allFinite()) return nullptr; + auto it = velocity_params.find(kf->id_); + if (it != velocity_params.end()) return it->second; + double* v = new double[3]; + v[0] = kf->velocity_.x(); + v[1] = kf->velocity_.y(); + v[2] = kf->velocity_.z(); + problem.AddParameterBlock(v, 3); + velocity_params[kf->id_] = v; + return v; + }; + + auto addBiasBlock = [&](std::map& sink, + const Keyframe::Ptr& kf, + const Vec3& init, + double anchor_sigma) -> double* { + if (!kf || !init.allFinite()) return nullptr; + auto it = sink.find(kf->id_); + if (it != sink.end()) return it->second; + double* b = new double[3]; + b[0] = init.x(); + b[1] = init.y(); + b[2] = init.z(); + problem.AddParameterBlock(b, 3); + sink[kf->id_] = b; + if (anchor_sigma > 0.0) { + const double anchor_weight = 1.0 / anchor_sigma; + problem.AddResidualBlock(BiasAnchorError::Create(anchor_weight), + nullptr, b); + ++bias_anchor_residual_count; + } + return b; + }; + + const double velocity_weight = 1.0 / velocity_sigma_m; + const double preint_pos_weight = 1.0 / velocity_sigma_m; + const double preint_vel_weight = 1.0 / velocity_vel_sigma; + // Rotation residual sigma in radians. Default ~0.05 rad (~3°) over a + // KF-gap is within a gyro-only prediction's short-interval noise. + // Tuneable via env; <=0 disables the rotation residual only. + double rot_sigma_rad = 0.05; + if (const char* env = std::getenv("SVSLAM_BA_PREINT_ROT_SIGMA_RAD")) { + char* end = nullptr; + const double parsed = std::strtod(env, &end); + if (end != env && std::isfinite(parsed)) { + rot_sigma_rad = parsed; + } + } + const double preint_rot_weight = + (rot_sigma_rad > 0.0) ? 1.0 / rot_sigma_rad : 0.0; + const Vec3 gravity_w(0.0, 0.0, -9.81); + + for (std::size_t i = 0; i + 1 < ordered_keyframes.size(); ++i) { + const auto& kf_i = ordered_keyframes[i]; + const auto& kf_j = ordered_keyframes[i + 1]; + if (!kf_i || !kf_j) continue; + if (!kf_i->has_velocity_) continue; + if (!kf_i->velocity_.allFinite()) continue; + + const double dt_pair = kf_j->timestamp_ - kf_i->timestamp_; + if (!(dt_pair > 0.0) || dt_pair > 1.0) continue; + + auto it_i = pose_params.find(kf_i->id_); + auto it_j = pose_params.find(kf_j->id_); + if (it_i == pose_params.end() || it_j == pose_params.end()) continue; + if (problem.IsParameterBlockConstant(it_i->second) && + problem.IsParameterBlockConstant(it_j->second)) { + continue; + } + + const bool span_ok = kf_j->prev_imu_span_ && kf_j->prev_imu_span_->valid && + kf_j->prev_imu_span_->from_kf_id == kf_i->id_ && + kf_j->prev_imu_span_->dt > 0.0 && + std::abs(kf_j->prev_imu_span_->dt - dt_pair) < 0.25 * dt_pair + 1e-3 && + kf_j->has_velocity_ && kf_j->velocity_.allFinite() && + g_preintegration_residual_enabled.load(std::memory_order_relaxed); + + if (span_ok) { + double* v_i_param = addVelocityBlock(kf_i); + double* v_j_param = addVelocityBlock(kf_j); + if (!v_i_param || !v_j_param) continue; + double* ba_i_param = addBiasBlock(accel_bias_params, kf_i, + kf_i->accel_bias_, + bias_accel_anchor_sigma); + double* ba_j_param = addBiasBlock(accel_bias_params, kf_j, + kf_j->accel_bias_, + bias_accel_anchor_sigma); + double* bg_i_param = addBiasBlock(gyro_bias_params, kf_i, + kf_i->gyro_bias_, + bias_gyro_anchor_sigma); + double* bg_j_param = addBiasBlock(gyro_bias_params, kf_j, + kf_j->gyro_bias_, + bias_gyro_anchor_sigma); + if (!ba_i_param || !ba_j_param || !bg_i_param || !bg_j_param) { + continue; + } + + const SE3& T_cb = kf_j->prev_imu_span_->T_cam_imu; + const Eigen::Quaterniond delta_R_q = + kf_j->prev_imu_span_->delta_R.unit_quaternion(); + ceres::CostFunction* cost = VelocityPreintegrationError::Create( + kf_j->prev_imu_span_->delta_p, + kf_j->prev_imu_span_->delta_v, + delta_R_q, + kf_j->prev_imu_span_->bias_accel, + kf_j->prev_imu_span_->dt, + gravity_w, + T_cb.unit_quaternion(), + T_cb.translation(), + preint_pos_weight, + preint_vel_weight, + preint_rot_weight); + problem.AddResidualBlock(cost, new ceres::HuberLoss(0.5), + it_i->second, it_j->second, + v_i_param, v_j_param, + ba_i_param); + ++preintegration_residual_count; + + // Bias random-walk priors tie consecutive KFs. Without them + // the per-KF anchors would be the only coupling and biases + // could jerk between frames. + if (bias_accel_rw_sigma > 0.0) { + const double rw_weight = 1.0 / bias_accel_rw_sigma; + problem.AddResidualBlock( + BiasRandomWalkError::Create(rw_weight), nullptr, + ba_i_param, ba_j_param); + ++bias_random_walk_residual_count; + } + if (bias_gyro_rw_sigma > 0.0) { + const double rw_weight = 1.0 / bias_gyro_rw_sigma; + problem.AddResidualBlock( + BiasRandomWalkError::Create(rw_weight), nullptr, + bg_i_param, bg_j_param); + ++bias_random_walk_residual_count; + } + } else { + ceres::CostFunction* cost = VelocityDeltaPriorError::Create( + kf_i->velocity_, dt_pair, velocity_weight); + problem.AddResidualBlock(cost, new ceres::HuberLoss(0.5), + it_i->second, it_j->second); + ++velocity_residual_count; + } + } + } + + if (preintegration_residual_count > 0) { + std::cout << "BA: Added " << preintegration_residual_count + << " IMU preintegration residuals (pos_sigma=" << velocity_sigma_m + << " m, vel_sigma=" << velocity_vel_sigma << " m/s)" << std::endl; + } + if (velocity_residual_count > 0) { + std::cout << "BA: Added " << velocity_residual_count + << " loose velocity prior residuals (sigma=" << velocity_sigma_m + << " m)" << std::endl; + } + if (bias_anchor_residual_count > 0 || bias_random_walk_residual_count > 0) { + std::cout << "BA: Added " << bias_anchor_residual_count + << " bias anchor + " << bias_random_walk_residual_count + << " bias random-walk residuals" << std::endl; + } + if (residual_count == 0 && depth_residual_count == 0) { for (auto& kv : pose_params) { delete[] kv.second; @@ -526,6 +785,15 @@ void Optimizer::bundleAdjustment(const std::vector& keyframes, for (auto& kv : point_params) { delete[] kv.second; } + for (auto& kv : velocity_params) { + delete[] kv.second; + } + for (auto& kv : accel_bias_params) { + delete[] kv.second; + } + for (auto& kv : gyro_bias_params) { + delete[] kv.second; + } return; } @@ -545,8 +813,26 @@ void Optimizer::bundleAdjustment(const std::vector& keyframes, // Update State for (auto& kf : keyframes) { kf->T_cw_ = se3FromParameterBlock(pose_params[kf->id_]); + auto vel_it = velocity_params.find(kf->id_); + if (vel_it != velocity_params.end()) { + Vec3 v(vel_it->second[0], vel_it->second[1], vel_it->second[2]); + if (v.allFinite()) { + kf->velocity_ = v; + kf->has_velocity_ = true; + } + } + auto ba_it = accel_bias_params.find(kf->id_); + if (ba_it != accel_bias_params.end()) { + Vec3 b(ba_it->second[0], ba_it->second[1], ba_it->second[2]); + if (b.allFinite()) kf->accel_bias_ = b; + } + auto bg_it = gyro_bias_params.find(kf->id_); + if (bg_it != gyro_bias_params.end()) { + Vec3 b(bg_it->second[0], bg_it->second[1], bg_it->second[2]); + if (b.allFinite()) kf->gyro_bias_ = b; + } } - + for (auto& lm : landmarks) { if (point_params.count(lm->id_)) { double* param = point_params[lm->id_]; @@ -570,6 +856,15 @@ void Optimizer::bundleAdjustment(const std::vector& keyframes, for (auto& kv : point_params) { delete[] kv.second; } + for (auto& kv : velocity_params) { + delete[] kv.second; + } + for (auto& kv : accel_bias_params) { + delete[] kv.second; + } + for (auto& kv : gyro_bias_params) { + delete[] kv.second; + } } void Optimizer::poseGraphOptimization(Map::Ptr map, diff --git a/src/backend/optimizer.h b/src/backend/optimizer.h index 3d52e33..f0e48e1 100644 --- a/src/backend/optimizer.h +++ b/src/backend/optimizer.h @@ -48,6 +48,289 @@ struct DepthPriorError { double weight; }; +// IMU preintegration residual between consecutive keyframes (Stage 0c.c/a). +// +// Forster-style 6-DoF (position + velocity) residual consuming frozen +// preintegration deltas (delta_p, delta_v, dt) computed between KF_i and +// KF_j at a reference accelerometer bias (bias_accel_ref). First-order +// bias Jacobians let BA re-estimate the accel bias without re-running +// preintegration: +// J_p_ba = -0.5 * dt^2 * I +// J_v_ba = -dt * I +// Gyro-bias effects on delta_p/delta_v are dropped (they require rotation +// Jacobians); the gyro bias is instead shaped by anchor and random-walk +// priors. Rotation is coupled by a 3-DoF residual on the log of the +// rotation error between the predicted q_wb_j and the observed one, so +// the full residual has 9 dimensions (pos + vel + rot). +struct VelocityPreintegrationError { + VelocityPreintegrationError(const Vec3& delta_p, + const Vec3& delta_v, + const Eigen::Quaterniond& delta_R, + const Vec3& bias_accel_ref, + double dt, + const Vec3& gravity_w, + const Eigen::Quaterniond& q_cam_imu, + const Vec3& t_cam_imu, + double pos_weight, + double vel_weight, + double rot_weight) + : dt_(dt), + pos_weight_(pos_weight), + vel_weight_(vel_weight), + rot_weight_(rot_weight) { + dp_[0] = delta_p.x(); dp_[1] = delta_p.y(); dp_[2] = delta_p.z(); + dv_[0] = delta_v.x(); dv_[1] = delta_v.y(); dv_[2] = delta_v.z(); + g_[0] = gravity_w.x(); g_[1] = gravity_w.y(); g_[2] = gravity_w.z(); + ba_ref_[0] = bias_accel_ref.x(); + ba_ref_[1] = bias_accel_ref.y(); + ba_ref_[2] = bias_accel_ref.z(); + const Eigen::Quaterniond dR = delta_R.normalized(); + q_dR_[0] = dR.w(); + q_dR_[1] = dR.x(); + q_dR_[2] = dR.y(); + q_dR_[3] = dR.z(); + // T_cam_imu rotation: q_cb s.t. p_cam = q_cb * p_body. For the + // preintegration residual we need q_wb = q_wc * q_cb, which is the + // body's world rotation — body-frame deltas then rotate into world + // via p_world = q_wb * p_body. + q_cb_[0] = q_cam_imu.w(); + q_cb_[1] = q_cam_imu.x(); + q_cb_[2] = q_cam_imu.y(); + q_cb_[3] = q_cam_imu.z(); + // camera origin expressed in body frame (t_bc). For T_cb = [q_cb | + // t_cb] (body→cam), T_bc = inverse gives t_bc = -q_cb^T * t_cb. + const Eigen::Quaterniond q_bc = q_cam_imu.conjugate(); + const Vec3 t_bc = -(q_bc * t_cam_imu); + t_bc_[0] = t_bc.x(); + t_bc_[1] = t_bc.y(); + t_bc_[2] = t_bc.z(); + } + + template + bool operator()(const T* const pose_i, // T_cw_i + const T* const pose_j, // T_cw_j + const T* const vel_i, // world-frame velocity of IMU body at KF_i + const T* const vel_j, // world-frame velocity of IMU body at KF_j + const T* const bias_accel_i, // accel bias at KF_i (m/s^2) + T* residuals) const { + // q_wc_i = conjugate(q_cw_i) + const T q_wc_i[4] = {pose_i[3], -pose_i[4], -pose_i[5], -pose_i[6]}; + const T neg_t_i[3] = {-pose_i[0], -pose_i[1], -pose_i[2]}; + T p_wc_i[3]; + ceres::QuaternionRotatePoint(q_wc_i, neg_t_i, p_wc_i); + + const T q_wc_j[4] = {pose_j[3], -pose_j[4], -pose_j[5], -pose_j[6]}; + const T neg_t_j[3] = {-pose_j[0], -pose_j[1], -pose_j[2]}; + T p_wc_j[3]; + ceres::QuaternionRotatePoint(q_wc_j, neg_t_j, p_wc_j); + + // q_wb_i = q_wc_i * q_cb (body-in-world rotation at KF_i). + const T q_cb[4] = {T(q_cb_[0]), T(q_cb_[1]), T(q_cb_[2]), T(q_cb_[3])}; + T q_wb_i[4]; + ceres::QuaternionProduct(q_wc_i, q_cb, q_wb_i); + T q_wb_j[4]; + ceres::QuaternionProduct(q_wc_j, q_cb, q_wb_j); + + // p_wb = p_wc - R_wb * t_bc (camera origin in body frame → world). + const T t_bc[3] = {T(t_bc_[0]), T(t_bc_[1]), T(t_bc_[2])}; + T R_wb_t_bc_i[3]; + ceres::QuaternionRotatePoint(q_wb_i, t_bc, R_wb_t_bc_i); + T R_wb_t_bc_j[3]; + ceres::QuaternionRotatePoint(q_wb_j, t_bc, R_wb_t_bc_j); + const T p_wb_i[3] = {p_wc_i[0] - R_wb_t_bc_i[0], + p_wc_i[1] - R_wb_t_bc_i[1], + p_wc_i[2] - R_wb_t_bc_i[2]}; + const T p_wb_j[3] = {p_wc_j[0] - R_wb_t_bc_j[0], + p_wc_j[1] - R_wb_t_bc_j[1], + p_wc_j[2] - R_wb_t_bc_j[2]}; + + const T dt = T(dt_); + const T half_dt2 = T(0.5) * dt * dt; + + // First-order accel-bias correction in the body-at-i frame. + const T dba[3] = { + bias_accel_i[0] - T(ba_ref_[0]), + bias_accel_i[1] - T(ba_ref_[1]), + bias_accel_i[2] - T(ba_ref_[2]) + }; + const T dp_corr[3] = { + T(dp_[0]) - half_dt2 * dba[0], + T(dp_[1]) - half_dt2 * dba[1], + T(dp_[2]) - half_dt2 * dba[2] + }; + const T dv_corr[3] = { + T(dv_[0]) - dt * dba[0], + T(dv_[1]) - dt * dba[1], + T(dv_[2]) - dt * dba[2] + }; + + // Rotate body-frame (bias-corrected) deltas into world via R_wb_i. + T R_dp[3]; + ceres::QuaternionRotatePoint(q_wb_i, dp_corr, R_dp); + T R_dv[3]; + ceres::QuaternionRotatePoint(q_wb_i, dv_corr, R_dv); + + residuals[0] = T(pos_weight_) * + ((p_wb_j[0] - p_wb_i[0]) - vel_i[0] * dt - T(g_[0]) * half_dt2 - R_dp[0]); + residuals[1] = T(pos_weight_) * + ((p_wb_j[1] - p_wb_i[1]) - vel_i[1] * dt - T(g_[1]) * half_dt2 - R_dp[1]); + residuals[2] = T(pos_weight_) * + ((p_wb_j[2] - p_wb_i[2]) - vel_i[2] * dt - T(g_[2]) * half_dt2 - R_dp[2]); + residuals[3] = T(vel_weight_) * + (vel_j[0] - vel_i[0] - T(g_[0]) * dt - R_dv[0]); + residuals[4] = T(vel_weight_) * + (vel_j[1] - vel_i[1] - T(g_[1]) * dt - R_dv[1]); + residuals[5] = T(vel_weight_) * + (vel_j[2] - vel_i[2] - T(g_[2]) * dt - R_dv[2]); + + // Rotation residual: q_wb_j_pred = q_wb_i * delta_R; compare to q_wb_j. + // error = q_wb_j_pred_inv * q_wb_j. For small errors, 2 * vector part + // ≈ log(error) (the so(3) tangent vector). No gyro-bias correction is + // applied yet; gyro bias stays under anchor + random-walk priors only. + const T q_dR[4] = {T(q_dR_[0]), T(q_dR_[1]), T(q_dR_[2]), T(q_dR_[3])}; + T q_wb_j_pred[4]; + ceres::QuaternionProduct(q_wb_i, q_dR, q_wb_j_pred); + const T q_pred_inv[4] = {q_wb_j_pred[0], -q_wb_j_pred[1], + -q_wb_j_pred[2], -q_wb_j_pred[3]}; + T q_err[4]; + ceres::QuaternionProduct(q_pred_inv, q_wb_j, q_err); + // Keep the linearized form so a perfect match gives exactly zero + // (the first-order expansion coincides with log for small errors). + residuals[6] = T(rot_weight_) * T(2.0) * q_err[1]; + residuals[7] = T(rot_weight_) * T(2.0) * q_err[2]; + residuals[8] = T(rot_weight_) * T(2.0) * q_err[3]; + return true; + } + + static ceres::CostFunction* Create(const Vec3& delta_p, + const Vec3& delta_v, + const Eigen::Quaterniond& delta_R, + const Vec3& bias_accel_ref, + double dt, + const Vec3& gravity_w, + const Eigen::Quaterniond& q_cam_imu, + const Vec3& t_cam_imu, + double pos_weight, + double vel_weight, + double rot_weight) { + return new ceres::AutoDiffCostFunction( + new VelocityPreintegrationError(delta_p, delta_v, delta_R, + bias_accel_ref, dt, + gravity_w, q_cam_imu, t_cam_imu, + pos_weight, vel_weight, rot_weight)); + } + + double dp_[3]; + double dv_[3]; + double g_[3]; + double ba_ref_[3]; + double q_cb_[4]; // w, x, y, z + double q_dR_[4]; // w, x, y, z (body-frame preintegrated rotation) + double t_bc_[3]; + double dt_; + double pos_weight_; + double vel_weight_; + double rot_weight_; +}; + +// Zero-anchor prior on an IMU bias (accel or gyro). Keeps the bias near +// origin with a soft pull, preventing it from drifting when only the +// weak preintegration residual applies. +struct BiasAnchorError { + explicit BiasAnchorError(double weight) : weight_(weight) {} + + template + bool operator()(const T* const bias, T* residuals) const { + residuals[0] = T(weight_) * bias[0]; + residuals[1] = T(weight_) * bias[1]; + residuals[2] = T(weight_) * bias[2]; + return true; + } + + static ceres::CostFunction* Create(double weight) { + return new ceres::AutoDiffCostFunction( + new BiasAnchorError(weight)); + } + + double weight_; +}; + +// Random-walk prior between consecutive keyframes' biases. Enforces the +// "bias varies slowly" assumption; smaller sigma → more coupling. +struct BiasRandomWalkError { + explicit BiasRandomWalkError(double weight) : weight_(weight) {} + + template + bool operator()(const T* const bias_i, + const T* const bias_j, + T* residuals) const { + residuals[0] = T(weight_) * (bias_j[0] - bias_i[0]); + residuals[1] = T(weight_) * (bias_j[1] - bias_i[1]); + residuals[2] = T(weight_) * (bias_j[2] - bias_i[2]); + return true; + } + + static ceres::CostFunction* Create(double weight) { + return new ceres::AutoDiffCostFunction( + new BiasRandomWalkError(weight)); + } + + double weight_; +}; + +// Loose IMU velocity prior between consecutive keyframes. +// +// Constrains the camera position delta in the world frame between two +// keyframes to match v_i_world * dt (ignoring 0.5 g dt^2 since sub-0.1s +// keyframe gaps give <5 cm gravity drop, well inside the loose sigma). +// +// Velocity itself is not a parameter block here: the IMU preintegration in +// Tracking writes KF_i->velocity_ ahead of BA and BA only uses it as a soft +// prior on pose translations. This is the "loose prior, not tight VIO" +// scaffolding for Stage 0b. +struct VelocityDeltaPriorError { + VelocityDeltaPriorError(const Vec3& v_i_world, double dt, double weight) + : dt_(dt), weight_(weight) { + v_wx_ = v_i_world.x(); + v_wy_ = v_i_world.y(); + v_wz_ = v_i_world.z(); + } + + template + bool operator()(const T* const pose_i, // [tx, ty, tz, qw, qx, qy, qz] for T_cw_i + const T* const pose_j, + T* residuals) const { + // p_wc = -R_cw^T * t_cw = rotate(-t_cw) by the inverse (conjugate) of q_cw. + const T q_i_inv[4] = {pose_i[3], -pose_i[4], -pose_i[5], -pose_i[6]}; + const T neg_t_i[3] = {-pose_i[0], -pose_i[1], -pose_i[2]}; + T p_wc_i[3]; + ceres::QuaternionRotatePoint(q_i_inv, neg_t_i, p_wc_i); + + const T q_j_inv[4] = {pose_j[3], -pose_j[4], -pose_j[5], -pose_j[6]}; + const T neg_t_j[3] = {-pose_j[0], -pose_j[1], -pose_j[2]}; + T p_wc_j[3]; + ceres::QuaternionRotatePoint(q_j_inv, neg_t_j, p_wc_j); + + const T dp_x = p_wc_j[0] - p_wc_i[0]; + const T dp_y = p_wc_j[1] - p_wc_i[1]; + const T dp_z = p_wc_j[2] - p_wc_i[2]; + + residuals[0] = T(weight_) * (dp_x - T(v_wx_) * T(dt_)); + residuals[1] = T(weight_) * (dp_y - T(v_wy_) * T(dt_)); + residuals[2] = T(weight_) * (dp_z - T(v_wz_) * T(dt_)); + return true; + } + + static ceres::CostFunction* Create(const Vec3& v_i_world, double dt, double weight) { + return new ceres::AutoDiffCostFunction( + new VelocityDeltaPriorError(v_i_world, dt, weight)); + } + + double v_wx_, v_wy_, v_wz_; + double dt_; + double weight_; +}; + // Gravity prior cost function: constrains roll/pitch by requiring that // R_cw * gravity_world ≈ gravity_camera (measured by accelerometer) // gravity_world = [0, 0, -1] after gravity alignment @@ -84,6 +367,19 @@ struct GravityPriorError { class Optimizer { public: + // Gate for the IMU preintegration residual in local BA. Set to true by + // Tracking once Visual-Inertial Initialization has estimated scale / + // gravity / biases / velocities; before that point BA falls back to + // the loose VelocityDeltaPriorError, because the preintegration + // residual built on uncalibrated biases can destabilize the solve on + // short / rotation-heavy sequences (e.g. EuRoC V1_01). + // + // The flag is a static toggle rather than a parameter so the existing + // Optimizer::bundleAdjustment call sites don't need plumbing changes. + // Thread-safety: set once on the tracking thread, read from BA on the + // local-mapping thread — a relaxed atomic is enough. + static void setPreintegrationResidualEnabled(bool on); + static bool preintegrationResidualEnabled(); struct PoseGraphEdge { Keyframe::Ptr from; Keyframe::Ptr to; diff --git a/src/core/frame.cc b/src/core/frame.cc index f980986..85fc956 100644 --- a/src/core/frame.cc +++ b/src/core/frame.cc @@ -49,4 +49,9 @@ void Frame::extractORB(const cv::Ptr& detector) { landmarks_.resize(keypoints_.size(), nullptr); } +std::vector> Frame::snapshotLandmarks() const { + std::lock_guard lock(mutex_); + return landmarks_; +} + } diff --git a/src/core/frame.h b/src/core/frame.h index 171869f..1e4a4a9 100644 --- a/src/core/frame.h +++ b/src/core/frame.h @@ -27,7 +27,16 @@ class Frame { // Pose: T_world_camera (Camera to World) or T_cw (World to Camera) // Let's use T_cw (World -> Camera) as is common in ORB-SLAM - SE3 T_cw_; + SE3 T_cw_; + + // VIO state (scaffolding for Stage 0b preintegration; not yet used in BA). + // velocity_ is in the world frame (m/s). accel_bias_ / gyro_bias_ are + // IMU biases in the sensor frame (m/s^2, rad/s). has_velocity_ is set + // once the tracking pipeline produces a usable velocity estimate. + Vec3 velocity_ = Vec3::Zero(); + Vec3 accel_bias_ = Vec3::Zero(); + Vec3 gyro_bias_ = Vec3::Zero(); + bool has_velocity_ = false; // Depth image (CV_16UC1 in mm for sensor depth, or CV_32FC1 in meters for DL depth) cv::Mat depth_image_; @@ -40,18 +49,27 @@ class Frame { // Back-project pixel with known depth to 3D world point Vec3 backprojectWithDepth(const cv::KeyPoint& kp, float depth_m) const; + // Snapshot landmarks_ under mutex_ for safe cross-thread iteration. + // Writers on tracking thread must take mutex_ around landmarks_ writes; + // readers on other threads (LocalMapping onBACompleted path) should call + // this instead of iterating landmarks_ directly. + std::vector> snapshotLandmarks() const; + // Features std::vector keypoints_; cv::Mat descriptors_; - // Map points associated with features + // Map points associated with features. + // Writes must be guarded by mutex_ once this Frame has been published as + // Tracking::current_frame_ (LocalMapping::onBACompleted reads it via the + // tracking BA callback). See snapshotLandmarks() for reads. std::vector> landmarks_; // Grid for fast search (optional, but good for requirements) // Skipping grid implementation for now to keep it minimal, // but reserving member if needed or just using brute force for now. - std::mutex mutex_; + mutable std::mutex mutex_; }; } diff --git a/src/core/keyframe.cc b/src/core/keyframe.cc index 7e12dc3..7ad7f1a 100644 --- a/src/core/keyframe.cc +++ b/src/core/keyframe.cc @@ -7,11 +7,17 @@ namespace svslam { Keyframe::Keyframe(Frame::Ptr frame) : id_(frame->id_), timestamp_(frame->timestamp_), camera_(frame->camera_), T_cw_(frame->getPose()), + velocity_(frame->velocity_), + accel_bias_(frame->accel_bias_), + gyro_bias_(frame->gyro_bias_), + has_velocity_(frame->has_velocity_), depth_image_(frame->depth_image_.empty() ? cv::Mat() : frame->depth_image_.clone()), depth_is_metric_(frame->depth_is_metric_), depth_is_learned_(frame->depth_is_learned_), keypoints_(frame->keypoints_), descriptors_(frame->descriptors_.clone()), - landmarks_(frame->landmarks_) + // Snapshot under frame->mutex_ so we don't copy from landmarks_ while + // onBACompleted on the LocalMapping thread holds the same mutex_. + landmarks_(frame->snapshotLandmarks()) { } diff --git a/src/core/keyframe.h b/src/core/keyframe.h index 12e55ad..ab66da6 100644 --- a/src/core/keyframe.h +++ b/src/core/keyframe.h @@ -1,7 +1,10 @@ #pragma once +#include + #include "core/common.h" #include "core/frame.h" +#include "sensors/imu_preintegration_span.h" namespace svslam { @@ -18,6 +21,18 @@ class Keyframe : public std::enable_shared_from_this { SE3 T_cw_; + // VIO state (scaffolding for Stage 0b). Copied from the source Frame at + // ctor time; future preintegration may update these during local BA. + Vec3 velocity_ = Vec3::Zero(); + Vec3 accel_bias_ = Vec3::Zero(); + Vec3 gyro_bias_ = Vec3::Zero(); + bool has_velocity_ = false; + + // Preintegration FROM the previous keyframe TO this one (Stage 0c). Null + // until the VIO path populates it — e.g. EuRoC mono with imu_buffer_ + // filled. Consumers must check valid == true before reading deltas. + std::unique_ptr prev_imu_span_; + // Depth image (copied from Frame) cv::Mat depth_image_; bool depth_is_metric_ = true; diff --git a/src/io/euroc_dataset.cc b/src/io/euroc_dataset.cc index 58f03ac..69ea028 100644 --- a/src/io/euroc_dataset.cc +++ b/src/io/euroc_dataset.cc @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -146,6 +147,30 @@ EurocDataset::EurocDataset(const std::string& seq_dir, return; } + // Cache cam0's T_BS (body=IMU → cam0) as a Sophus SE3 for downstream + // preintegration consumers. EuRoC matrices are stored row-major. + if (cam0_calib.T_BS.size() == 16) { + Eigen::Matrix4d T_cam_imu_mat; + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + T_cam_imu_mat(r, c) = cam0_calib.T_BS[static_cast(r * 4 + c)]; + } + } + // Renormalize the rotation block so SE3's internal quaternion stays + // unit-length despite the YAML's 6-digit truncation. + Eigen::Matrix3d R = T_cam_imu_mat.block<3, 3>(0, 0); + Eigen::JacobiSVD svd( + R, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix3d R_ortho = svd.matrixU() * svd.matrixV().transpose(); + if (R_ortho.determinant() < 0.0) { + Eigen::Matrix3d V = svd.matrixV(); + V.col(2) *= -1.0; + R_ortho = svd.matrixU() * V.transpose(); + } + cam0_from_imu_ = SE3(R_ortho, T_cam_imu_mat.block<3, 1>(0, 3)); + has_cam0_from_imu_ = true; + } + initCalibration(cam0_calib, K_, dist_coeffs_, new_K_, undist_map1_, undist_map2_); if (stereo_enabled_) { initCalibration(cam1_calib, right_K_, right_dist_coeffs_, right_new_K_, right_undist_map1_, @@ -170,6 +195,15 @@ EurocDataset::EurocDataset(const std::string& seq_dir, error_ = stereo_enabled_ ? "no stereo entries in data.csv" : "no entries in data.csv"; return; } + + // IMU is optional — only fail on parse error, not on missing file. + const std::string imu_csv = + (std::filesystem::path(seq_dir_) / "mav0" / "imu0" / "data.csv").string(); + if (std::filesystem::exists(imu_csv)) { + if (!loadImuCsv(imu_csv)) { + return; + } + } } bool EurocDataset::isValid() const { return error_.empty(); } @@ -220,6 +254,32 @@ bool EurocDataset::loadSensorYaml(const std::string& sensor_yaml_path, EurocPinh return false; } + // Slurp the whole file so we can fold multi-line YAML arrays (EuRoC + // stores T_BS's 16 values wrapped across 4 lines) into a single virtual + // line before handing it to parseArrayLine. + std::string raw((std::istreambuf_iterator(ifs)), + std::istreambuf_iterator()); + + // Collapse any '[' ... ']' block into one line. Safe here because + // sensor.yaml only uses bracket arrays for numeric sequences. + std::string flat; + flat.reserve(raw.size()); + int bracket_depth = 0; + for (char c : raw) { + if (c == '[') { + ++bracket_depth; + flat.push_back(c); + } else if (c == ']') { + --bracket_depth; + flat.push_back(c); + } else if (bracket_depth > 0 && (c == '\n' || c == '\r')) { + flat.push_back(' '); + } else { + flat.push_back(c); + } + } + + std::stringstream folded(flat); bool got_intrinsics = false; bool got_resolution = false; std::vector values; @@ -227,7 +287,7 @@ bool EurocDataset::loadSensorYaml(const std::string& sensor_yaml_path, EurocPinh bool in_t_bs_block = false; std::string line; - while (std::getline(ifs, line)) { + while (std::getline(folded, line)) { line = trim(line); if (line.empty()) continue; if (line[0] == '#') continue; @@ -336,6 +396,68 @@ bool EurocDataset::loadDataCsv(const std::string& data_csv_path, return true; } +bool EurocDataset::loadImuCsv(const std::string& imu_csv_path) { + std::ifstream ifs(imu_csv_path); + if (!ifs.is_open()) { + error_ = "failed to open imu0 data.csv: " + imu_csv_path; + return false; + } + + imu_entries_.clear(); + std::string line; + bool first = true; + while (std::getline(ifs, line)) { + line = trim(line); + if (line.empty()) continue; + if (line[0] == '#') continue; + if (first) { + first = false; + if (line.find("timestamp") != std::string::npos) continue; + } + + std::stringstream ss(line); + std::string field; + std::vector fields; + while (std::getline(ss, field, ',')) { + fields.push_back(trim(field)); + } + // Expect: ts_ns, wx, wy, wz, ax, ay, az + if (fields.size() < 7) continue; + + try { + const long long ts_ns = std::stoll(fields[0]); + ImuEntry e; + e.timestamp_sec = static_cast(ts_ns) * 1e-9; + e.gyro = Vec3(std::stod(fields[1]), std::stod(fields[2]), std::stod(fields[3])); + e.accel = Vec3(std::stod(fields[4]), std::stod(fields[5]), std::stod(fields[6])); + imu_entries_.push_back(e); + } catch (const std::exception&) { + continue; + } + } + + std::sort(imu_entries_.begin(), imu_entries_.end(), + [](const ImuEntry& a, const ImuEntry& b) { + return a.timestamp_sec < b.timestamp_sec; + }); + return true; +} + +std::vector EurocDataset::getImuBetween(double t0, double t1) const { + std::vector out; + if (imu_entries_.empty() || !(t1 > t0)) { + return out; + } + // Binary search for first sample with timestamp > t0. + auto lo = std::upper_bound( + imu_entries_.begin(), imu_entries_.end(), t0, + [](double v, const ImuEntry& e) { return v < e.timestamp_sec; }); + for (auto it = lo; it != imu_entries_.end() && it->timestamp_sec <= t1; ++it) { + out.push_back(*it); + } + return out; +} + bool EurocDataset::buildStereoEntries(const std::vector& left_entries, const std::vector& right_entries) { entries_.clear(); diff --git a/src/io/euroc_dataset.h b/src/io/euroc_dataset.h index 3c62575..3340e8e 100644 --- a/src/io/euroc_dataset.h +++ b/src/io/euroc_dataset.h @@ -4,7 +4,9 @@ #include #include +#include "core/common.h" #include "io/euroc_pinhole_calibration.h" +#include "sensors/imu.h" namespace svslam { @@ -31,6 +33,18 @@ class EurocDataset { bool next(cv::Mat& image, double& timestamp_sec); bool next(cv::Mat& left_image, cv::Mat& right_image, double& timestamp_sec); + // IMU accessors (empty vector if mav0/imu0/data.csv is absent). + bool hasImu() const { return !imu_entries_.empty(); } + const std::vector& allImu() const { return imu_entries_; } + // Returns IMU samples strictly within (t0, t1]. Inputs in seconds. + std::vector getImuBetween(double t0, double t1) const; + + // Extrinsic for cam0: T_cam_imu (transforms IMU-body-frame points to + // cam0 frame). EuRoC sensor.yaml stores this as T_BS with body := IMU. + // Returns identity if cam0.T_BS was not parsed. + SE3 cam0FromImuExtrinsic() const { return cam0_from_imu_; } + bool hasCam0FromImuExtrinsic() const { return has_cam0_from_imu_; } + private: struct CsvEntry { long long timestamp_ns; @@ -42,6 +56,9 @@ class EurocDataset { bool loadSensorYaml(const std::string& sensor_yaml_path, EurocPinholeCalibration::Camera& calib); bool loadDataCsv(const std::string& data_csv_path, const std::string& data_dir, std::vector& entries); + // Populates imu_entries_ from mav0/imu0/data.csv. Returns true on success + // or if file is absent (IMU is optional). False only on parse error. + bool loadImuCsv(const std::string& imu_csv_path); bool buildStereoEntries(const std::vector& left_entries, const std::vector& right_entries); void buildMonoEntries(const std::vector& left_entries); void initCalibration(const EurocPinholeCalibration::Camera& calib, @@ -70,9 +87,13 @@ class EurocDataset { cv::Mat right_undist_map2_; std::vector entries_; + std::vector imu_entries_; size_t index_ = 0; bool stereo_enabled_ = false; double stereo_baseline_meters_ = 0.0; + + SE3 cam0_from_imu_; // defaults to identity + bool has_cam0_from_imu_ = false; }; } // namespace svslam diff --git a/src/sensors/imu.h b/src/sensors/imu.h new file mode 100644 index 0000000..bd5ce0f --- /dev/null +++ b/src/sensors/imu.h @@ -0,0 +1,15 @@ +#pragma once + +#include "core/common.h" + +namespace svslam { + +// A single IMU measurement (accel + gyro) at a given timestamp. +// Units: m/s^2 for accel, rad/s for gyro. Timestamp in seconds. +struct ImuEntry { + double timestamp_sec = 0.0; + Vec3 accel = Vec3::Zero(); + Vec3 gyro = Vec3::Zero(); +}; + +} // namespace svslam diff --git a/src/sensors/imu_preintegration_span.h b/src/sensors/imu_preintegration_span.h new file mode 100644 index 0000000..4da14c6 --- /dev/null +++ b/src/sensors/imu_preintegration_span.h @@ -0,0 +1,34 @@ +#pragma once + +#include + +#include "core/common.h" + +namespace svslam { + +// Frozen preintegration between two consecutive keyframes. The holder (a +// Keyframe) owns the span FROM the previous KF TO itself, so `from_kf_id` +// points at the predecessor and the KF's own id is the target. +// +// delta_R / delta_v / delta_p are all expressed in the from-KF's IMU-body +// frame at the bias snapshot recorded in bias_accel / bias_gyro. Future +// bias updates can be applied linearly on top of the stored deltas when a +// VIO backend wires in bias Jacobians; Stage 0c keeps biases fixed at the +// values used for integration. +struct ImuPreintegrationSpan { + Sophus::SO3d delta_R; // rotation accumulated i->j in body frame + Vec3 delta_v = Vec3::Zero(); // velocity delta in body-at-i frame + Vec3 delta_p = Vec3::Zero(); // position delta in body-at-i frame + double dt = 0.0; // integration duration in seconds + Vec3 bias_accel = Vec3::Zero(); // accel bias used during integration + Vec3 bias_gyro = Vec3::Zero(); // gyro bias used during integration + unsigned long from_kf_id = 0; // predecessor keyframe id + bool valid = false; // false until populated + sanity-checked + + // Camera-from-IMU rigid transform captured at span-creation time. Used + // by BA to translate body-frame deltas into camera-frame pose + // constraints. Identity when the dataset didn't provide an extrinsic. + SE3 T_cam_imu; +}; + +} // namespace svslam diff --git a/src/sensors/imu_preintegrator.h b/src/sensors/imu_preintegrator.h new file mode 100644 index 0000000..8232829 --- /dev/null +++ b/src/sensors/imu_preintegrator.h @@ -0,0 +1,89 @@ +#pragma once + +#include + +#include "core/common.h" +#include "sensors/imu.h" + +namespace svslam { + +// Minimal Forster-style IMU preintegration. +// Accumulates delta_R, delta_v, delta_p over an interval [i, j] given the +// IMU biases at i. Supports predicting the keyframe-j state from the +// keyframe-i state plus the gravity vector in the world frame. +// +// This class is intentionally scaffolding-level: bias Jacobians and noise +// covariance propagation are left out for a follow-up commit when a VIO +// residual is actually wired into BA. +class ImuPreintegrator { +public: + using SO3 = Sophus::SO3d; + + ImuPreintegrator() { reset(Vec3::Zero(), Vec3::Zero()); } + ImuPreintegrator(const Vec3& accel_bias, const Vec3& gyro_bias) { + reset(accel_bias, gyro_bias); + } + + // Discard any accumulated state and re-anchor with the given biases. + void reset(const Vec3& accel_bias, const Vec3& gyro_bias) { + delta_R_ = SO3(); + delta_v_ = Vec3::Zero(); + delta_p_ = Vec3::Zero(); + dt_ = 0.0; + accel_bias_ = accel_bias; + gyro_bias_ = gyro_bias; + } + + // Integrate one IMU sample over a dt second interval using midpoint-free + // (Euler) integration in the i-frame. Callers are responsible for + // selecting dt from the IMU timestamps. + void integrate(const Vec3& accel, const Vec3& gyro, double dt) { + if (!(dt > 0.0)) { + return; + } + const Vec3 a_unbiased = accel - accel_bias_; + const Vec3 w_unbiased = gyro - gyro_bias_; + + // Position and velocity increments must be evaluated BEFORE updating + // delta_R_ so the integrator uses the start-of-interval rotation. + const Vec3 accel_i = delta_R_ * a_unbiased; + delta_p_ += delta_v_ * dt + 0.5 * accel_i * dt * dt; + delta_v_ += accel_i * dt; + + // Right-multiply rotation with Exp(w dt). + delta_R_ = delta_R_ * SO3::exp(w_unbiased * dt); + + dt_ += dt; + } + + // Predict keyframe-j state from keyframe-i state (world frame) and + // gravity vector (world frame, m/s^2; e.g. Vec3(0, 0, -9.81) for TUM). + void predict(const SO3& R_i, + const Vec3& v_i, + const Vec3& p_i, + const Vec3& gravity_w, + SO3& R_j, + Vec3& v_j, + Vec3& p_j) const { + R_j = R_i * delta_R_; + v_j = v_i + gravity_w * dt_ + R_i * delta_v_; + p_j = p_i + v_i * dt_ + 0.5 * gravity_w * dt_ * dt_ + R_i * delta_p_; + } + + const SO3& deltaR() const { return delta_R_; } + const Vec3& deltaV() const { return delta_v_; } + const Vec3& deltaP() const { return delta_p_; } + double deltaT() const { return dt_; } + const Vec3& accelBias() const { return accel_bias_; } + const Vec3& gyroBias() const { return gyro_bias_; } + +private: + SO3 delta_R_; + Vec3 delta_v_ = Vec3::Zero(); + Vec3 delta_p_ = Vec3::Zero(); + double dt_ = 0.0; + Vec3 accel_bias_ = Vec3::Zero(); + Vec3 gyro_bias_ = Vec3::Zero(); +}; + +} // namespace svslam diff --git a/src/tracking/tracking.cc b/src/tracking/tracking.cc index 98b6ddb..47c0c59 100644 --- a/src/tracking/tracking.cc +++ b/src/tracking/tracking.cc @@ -14,6 +14,8 @@ #include "core/keyframe.h" #include "core/landmark.h" #include "sensors/accelerometer.h" +#include "sensors/imu_preintegrator.h" +#include "backend/optimizer.h" namespace svslam { @@ -238,6 +240,10 @@ void assignFrameLandmarksFromInliers(const Frame::Ptr& frame, return; } + // Hold frame->mutex_ around the assign + writes: onBACompleted may be + // snapshotting frame->landmarks_ on the LocalMapping thread at the same + // time. + std::lock_guard lock(frame->mutex_); frame->landmarks_.assign(frame->keypoints_.size(), nullptr); for (const int index : inliers) { if (index < 0 || index >= static_cast(keypoint_indices.size()) || @@ -316,6 +322,15 @@ bool Tracking::initializeWithDepth() { if (!gravity_aligned_ && !accel_buffer_.empty()) { Vec3 gravity = AccelerometerProcessor::estimateGravity(accel_buffer_); if (gravity.norm() > 0.5) { + // Accelerometer samples live in the IMU body frame; if T_cam_imu + // is set (EuRoC), rotate into the camera frame before building + // the align transform so the computed R_align lives in the + // camera-from-world sense that setPose(T_cw) expects. On + // sensors where IMU ≈ camera (TUM freiburg1), T_cam_imu_ is + // identity and this is a no-op. + if (has_cam_imu_extrinsic_) { + gravity = T_cam_imu_.so3() * gravity; + } Mat33 R_align = AccelerometerProcessor::computeGravityAlignment(gravity); SE3 T_aligned(R_align, Vec3(0, 0, 0)); current_frame_->setPose(T_aligned); @@ -330,6 +345,10 @@ bool Tracking::initializeWithDepth() { int created = 0; static unsigned long depth_lm_id = 200000; + // kf is not yet published to the map, so kf->landmarks_ writes are + // single-threaded. current_frame_ is published; hold its mutex_ across + // the loop to avoid racing with onBACompleted reads. + std::lock_guard frame_lock(current_frame_->mutex_); for (size_t i = 0; i < kf->keypoints_.size(); ++i) { auto lm = createDepthLandmark(kf, i, depth_lm_id); if (!lm) { @@ -460,6 +479,9 @@ bool Tracking::initialize() { if (!gravity_aligned_ && !accel_buffer_.empty()) { Vec3 gravity = AccelerometerProcessor::estimateGravity(accel_buffer_); if (gravity.norm() > 0.5) { + if (has_cam_imu_extrinsic_) { + gravity = T_cam_imu_.so3() * gravity; + } Mat33 R_align = AccelerometerProcessor::computeGravityAlignment(gravity); T_align = SE3(R_align, Vec3(0, 0, 0)); gravity_aligned_ = true; @@ -540,10 +562,19 @@ bool Tracking::initialize() { // Add landmarks to keyframes kf_init->landmarks_[idx_ref] = lm; kf_cur->landmarks_[idx_cur] = lm; - - // Update Frames as well so they are tracked - initial_frame_->landmarks_[idx_ref] = lm; - current_frame_->landmarks_[idx_cur] = lm; + + // Update Frames as well so they are tracked. Both frames + // are published (initial_frame_ may still be the target + // of an onBACompleted snapshot path), so take each + // mutex_ around the single-slot write. + { + std::lock_guard lock(initial_frame_->mutex_); + initial_frame_->landmarks_[idx_ref] = lm; + } + { + std::lock_guard lock(current_frame_->mutex_); + current_frame_->landmarks_[idx_cur] = lm; + } // Add to map if (map_) { @@ -643,6 +674,11 @@ bool Tracking::track() { SE3 T_cw_pred = velocity_ * last_frame_->getPose(); current_frame_->setPose(T_cw_pred); } + + // IMU preintegration -> world-frame velocity estimate on current_frame_. + // Pose uses the visual motion model above; IMU only seeds Frame::velocity_ + // so BA can apply a loose velocity prior. See predictVelocityFromImu(). + predictVelocityFromImu(); } // 2. Track Reference Keyframe (Frame-to-Frame matching for now) @@ -683,6 +719,7 @@ bool Tracking::track() { loop_correction_state_.skip_velocity_update_once = false; } else if (last_frame_) { velocity_ = current_frame_->getPose() * last_frame_->getPose().inverse(); + reconcileVelocityWithVisual(); } } else { std::cout << "Tracking: Lost, attempting relocalization..." << std::endl; @@ -755,6 +792,11 @@ bool Tracking::track() { // Create new Keyframe auto kf = std::make_shared(current_frame_); setKeyframeGravity(kf); + // Capture IMU preintegration from the previous reference KF. BA reads + // kf->prev_imu_span_ when the span is valid; otherwise it falls back + // to the loose velocity prior. reference_keyframe_ has not yet been + // updated to this kf, so it points at the correct predecessor. + populateKeyframeImuSpan(kf, reference_keyframe_); for (size_t i = 0; i < kf->landmarks_.size(); ++i) { auto& lm = kf->landmarks_[i]; if (!lm || lm->isBad()) continue; @@ -814,6 +856,11 @@ bool Tracking::track() { } loop_correction_state_.force_keyframe_insertion_once = false; + + // After each new KF, try to bootstrap VI init. Once it succeeds + // the call is a no-op and the BA preintegration residual becomes + // active (see gating in backend/optimizer.cc). + tryVisualInertialInit(); } return state_ == TrackingState::OK; @@ -826,8 +873,15 @@ bool Tracking::needNewKeyframe() { const int frames_since_reference = static_cast(current_frame_->id_ - reference_keyframe_->id_); + // Snapshot reference_keyframe_->landmarks_ under kf->mutex_ — + // LocalMapping::createNewMapPoints writes the same container. + std::vector ref_landmarks_snapshot; + { + std::lock_guard lock(reference_keyframe_->mutex_); + ref_landmarks_snapshot = reference_keyframe_->landmarks_; + } int ref_landmarks = 0; - for (auto& lm : reference_keyframe_->landmarks_) { + for (auto& lm : ref_landmarks_snapshot) { if (lm && !lm->isBad()) { ref_landmarks++; } @@ -1648,7 +1702,9 @@ bool Tracking::trackReferenceKeyframe() { // Propagate landmark associations from last frame via feature matches. // This is critical for bootstrapping 3D-2D PnP in subsequent frames. - current_frame_->landmarks_.assign(current_frame_->keypoints_.size(), nullptr); + // Hold current_frame_->mutex_ across assign + inner writes: onBACompleted + // may snapshot landmarks_ concurrently. last_frame_ is only read and + // only the tracking thread writes it, so reads below are safe. int propagated = 0; // Optimization: Pose from 3D-2D @@ -1656,37 +1712,42 @@ bool Tracking::trackReferenceKeyframe() { std::vector image_points; std::vector current_kp_indices; std::vector propagated_landmarks; - - for (const auto& m : good_matches) { - // Query is current, Train is last - int idx_last = m.trainIdx; - int idx_curr = m.queryIdx; - - if (idx_last >= 0 && idx_last < static_cast(last_frame_->landmarks_.size()) && - last_frame_->landmarks_[idx_last]) { - // Found a map point - Vec3 pos = last_frame_->landmarks_[idx_last]->getPos(); - if (!std::isfinite(pos.x()) || !std::isfinite(pos.y()) || !std::isfinite(pos.z())) continue; - Vec3 p_c = current_frame_->getPose() * pos; - if (!std::isfinite(p_c.x()) || !std::isfinite(p_c.y()) || !std::isfinite(p_c.z())) continue; - if (p_c[2] <= kMinTrackedDepthMeters || p_c[2] > kMaxTrackedDepthMeters) continue; + { + std::lock_guard lock(current_frame_->mutex_); + current_frame_->landmarks_.assign(current_frame_->keypoints_.size(), nullptr); - const int octave = current_frame_->keypoints_[idx_curr].octave; - const double gate_px = 48.0 * (1.0 + 0.12 * static_cast(std::max(0, octave))); - Vec2 proj = current_frame_->camera_->project(p_c); - const auto& uv = current_frame_->keypoints_[idx_curr].pt; - const double dx = uv.x - proj[0]; - const double dy = uv.y - proj[1]; - if ((dx * dx + dy * dy) > gate_px * gate_px) continue; + for (const auto& m : good_matches) { + // Query is current, Train is last + int idx_last = m.trainIdx; + int idx_curr = m.queryIdx; - object_points.push_back(cv::Point3f(pos.x(), pos.y(), pos.z())); - image_points.push_back(uv); + if (idx_last >= 0 && idx_last < static_cast(last_frame_->landmarks_.size()) && + last_frame_->landmarks_[idx_last]) { + // Found a map point + Vec3 pos = last_frame_->landmarks_[idx_last]->getPos(); + if (!std::isfinite(pos.x()) || !std::isfinite(pos.y()) || !std::isfinite(pos.z())) continue; + + Vec3 p_c = current_frame_->getPose() * pos; + if (!std::isfinite(p_c.x()) || !std::isfinite(p_c.y()) || !std::isfinite(p_c.z())) continue; + if (p_c[2] <= kMinTrackedDepthMeters || p_c[2] > kMaxTrackedDepthMeters) continue; + + const int octave = current_frame_->keypoints_[idx_curr].octave; + const double gate_px = 48.0 * (1.0 + 0.12 * static_cast(std::max(0, octave))); + Vec2 proj = current_frame_->camera_->project(p_c); + const auto& uv = current_frame_->keypoints_[idx_curr].pt; + const double dx = uv.x - proj[0]; + const double dy = uv.y - proj[1]; + if ((dx * dx + dy * dy) > gate_px * gate_px) continue; - current_frame_->landmarks_[idx_curr] = last_frame_->landmarks_[idx_last]; - current_kp_indices.push_back(idx_curr); - propagated_landmarks.push_back(last_frame_->landmarks_[idx_last]); - propagated++; + object_points.push_back(cv::Point3f(pos.x(), pos.y(), pos.z())); + image_points.push_back(uv); + + current_frame_->landmarks_[idx_curr] = last_frame_->landmarks_[idx_last]; + current_kp_indices.push_back(idx_curr); + propagated_landmarks.push_back(last_frame_->landmarks_[idx_last]); + propagated++; + } } } @@ -1844,8 +1905,12 @@ std::size_t Tracking::countValidFrameLandmarks(const Frame::Ptr& frame) { return 0; } + // Snapshot under frame->mutex_ — this path is reached from onBACompleted + // on the LocalMapping thread while the tracking thread may concurrently + // write frame->landmarks_[i]. + const auto snapshot = frame->snapshotLandmarks(); std::size_t correspondences = 0; - for (const auto& lm : frame->landmarks_) { + for (const auto& lm : snapshot) { if (!lm || lm->isBad()) continue; const Vec3 pos = lm->getPos(); if (!std::isfinite(pos.x()) || !std::isfinite(pos.y()) || !std::isfinite(pos.z())) { @@ -1925,13 +1990,16 @@ bool Tracking::recomputeCurrentPose() { return false; } - // Collect 3D-2D correspondences from current frame's landmark associations + // Collect 3D-2D correspondences from current frame's landmark associations. + // Snapshot landmarks_ under current_frame_->mutex_ — this runs on the + // LocalMapping thread via onBACompleted while tracking may be writing. + const auto landmarks_snapshot = current_frame_->snapshotLandmarks(); std::vector pts3d; std::vector pts2d; std::vector indices; - for (size_t i = 0; i < current_frame_->landmarks_.size(); i++) { - auto lm = current_frame_->landmarks_[i]; + for (size_t i = 0; i < landmarks_snapshot.size(); i++) { + auto lm = landmarks_snapshot[i]; if (!lm || lm->isBad()) continue; Vec3 pos = lm->getPos(); // BA-updated position @@ -2063,11 +2131,19 @@ bool Tracking::relocalize() { } } + // Snapshot kf->landmarks_ under kf->mutex_ to avoid racing with + // LocalMapping::createNewMapPoints writes on the same container. + std::vector kf_landmarks_snapshot; + { + std::lock_guard lock(kf->mutex_); + kf_landmarks_snapshot = kf->landmarks_; + } + int valid_3d_matches = 0; for (const auto& m : good) { const int kf_idx = m.trainIdx; - if (kf_idx < 0 || kf_idx >= static_cast(kf->landmarks_.size())) continue; - const auto& lm = kf->landmarks_[kf_idx]; + if (kf_idx < 0 || kf_idx >= static_cast(kf_landmarks_snapshot.size())) continue; + const auto& lm = kf_landmarks_snapshot[kf_idx]; if (!lm || lm->isBad()) continue; const Vec3 pos = lm->getPos(); @@ -2237,14 +2313,23 @@ bool Tracking::relocalize() { std::vector pts2d; std::vector match_indices; + // Snapshot cand.kf->landmarks_ under kf->mutex_ — LocalMapping's + // createNewMapPoints writes kf->landmarks_[idx] concurrently and TSan + // flagged the unprotected read here. + std::vector kf_landmarks; + { + std::lock_guard lock(cand.kf->mutex_); + kf_landmarks = cand.kf->landmarks_; + } + for (size_t m_idx = 0; m_idx < cand.matches.size(); m_idx++) { auto& m = cand.matches[m_idx]; int kf_idx = m.trainIdx; int curr_idx = m.queryIdx; - if (kf_idx >= 0 && kf_idx < static_cast(cand.kf->landmarks_.size()) && - cand.kf->landmarks_[kf_idx]) { - auto lm = cand.kf->landmarks_[kf_idx]; + if (kf_idx >= 0 && kf_idx < static_cast(kf_landmarks.size()) && + kf_landmarks[kf_idx]) { + auto lm = kf_landmarks[kf_idx]; if (lm->isBad()) continue; Vec3 pos = lm->getPos(); @@ -2356,20 +2441,35 @@ bool Tracking::relocalize() { const auto& best_candidate = candidates[best_success.candidate_index]; current_frame_->setPose(best_success.pose); - current_frame_->landmarks_.assign(current_frame_->keypoints_.size(), nullptr); - for (int idx : best_success.inlier_indices) { - if (idx < 0 || idx >= static_cast(best_success.match_indices.size())) { - continue; - } - const int orig_match_idx = best_success.match_indices[idx]; - if (orig_match_idx < 0 || orig_match_idx >= static_cast(best_candidate.matches.size())) { - continue; - } - const int kf_idx = best_candidate.matches[orig_match_idx].trainIdx; - const int curr_idx = best_candidate.matches[orig_match_idx].queryIdx; - if (kf_idx >= 0 && kf_idx < static_cast(best_candidate.kf->landmarks_.size()) && - curr_idx >= 0 && curr_idx < static_cast(current_frame_->landmarks_.size())) { - current_frame_->landmarks_[curr_idx] = best_candidate.kf->landmarks_[kf_idx]; + // Snapshot best_candidate.kf->landmarks_ first (separate lock) so we + // don't hold two container mutexes at once — LocalMapping only takes + // one KF mutex at a time, and the existing race-fix convention avoids + // lock-order inversion. + std::vector best_kf_landmarks; + { + std::lock_guard lock(best_candidate.kf->mutex_); + best_kf_landmarks = best_candidate.kf->landmarks_; + } + + // Lock current_frame_->mutex_ around assign + inner writes to avoid + // racing with onBACompleted snapshot on the LocalMapping thread. + { + std::lock_guard lock(current_frame_->mutex_); + current_frame_->landmarks_.assign(current_frame_->keypoints_.size(), nullptr); + for (int idx : best_success.inlier_indices) { + if (idx < 0 || idx >= static_cast(best_success.match_indices.size())) { + continue; + } + const int orig_match_idx = best_success.match_indices[idx]; + if (orig_match_idx < 0 || orig_match_idx >= static_cast(best_candidate.matches.size())) { + continue; + } + const int kf_idx = best_candidate.matches[orig_match_idx].trainIdx; + const int curr_idx = best_candidate.matches[orig_match_idx].queryIdx; + if (kf_idx >= 0 && kf_idx < static_cast(best_kf_landmarks.size()) && + curr_idx >= 0 && curr_idx < static_cast(current_frame_->landmarks_.size())) { + current_frame_->landmarks_[curr_idx] = best_kf_landmarks[kf_idx]; + } } } @@ -2498,8 +2598,14 @@ bool Tracking::reinitialize() { kf_ref->landmarks_[tp.idx_ref] = lm; kf_cur->landmarks_[tp.idx_cur] = lm; - reinitialization_state_.reference_frame->landmarks_[tp.idx_ref] = lm; - current_frame_->landmarks_[tp.idx_cur] = lm; + { + std::lock_guard lock(reinitialization_state_.reference_frame->mutex_); + reinitialization_state_.reference_frame->landmarks_[tp.idx_ref] = lm; + } + { + std::lock_guard lock(current_frame_->mutex_); + current_frame_->landmarks_[tp.idx_cur] = lm; + } if (map_) { map_->addLandmark(lm); @@ -2553,4 +2659,391 @@ void Tracking::setKeyframeGravity(Keyframe::Ptr kf) { kf->has_gravity_ = true; } +void Tracking::predictVelocityFromImu() { + if (!last_frame_ || !current_frame_) return; + if (imu_buffer_.empty()) return; + if (!gravity_aligned_) return; + + const double t_i = last_frame_->timestamp_; + const double t_j = current_frame_->timestamp_; + if (!(t_j > t_i) || (t_j - t_i) > 1.0) return; + + // Pick IMU samples whose timestamp falls in [t_i, t_j]. imu_buffer_ is + // already sorted ascending at load time (EuRoC imu0 CSV order). + auto begin_it = std::lower_bound( + imu_buffer_.begin(), imu_buffer_.end(), t_i, + [](const ImuEntry& e, double t) { return e.timestamp_sec < t; }); + auto end_it = std::upper_bound( + imu_buffer_.begin(), imu_buffer_.end(), t_j, + [](double t, const ImuEntry& e) { return t < e.timestamp_sec; }); + if (std::distance(begin_it, end_it) < 2) return; + + // Biases are zero in Stage 0b until a VIO backend estimates them; keep the + // last-frame copy so upstream code can refine them later without touching + // this call site. + ImuPreintegrator preint(last_frame_->accel_bias_, last_frame_->gyro_bias_); + auto prev_it = begin_it; + auto next_it = prev_it + 1; + while (next_it != end_it) { + const double dt_step = next_it->timestamp_sec - prev_it->timestamp_sec; + if (dt_step > 0.0 && dt_step < 0.5) { + // Midpoint-free: integrate using the earlier sample; fine for a + // loose prior at EuRoC's 200 Hz IMU rate. + preint.integrate(prev_it->accel, prev_it->gyro, dt_step); + } + prev_it = next_it; + ++next_it; + } + if (preint.deltaT() <= 0.0) return; + + // Convert the camera pose T_cw to an IMU-body pose T_wb so the Forster + // integrator runs in the frame it measures in. With T_cam_imu_ unset the + // formula collapses to the camera-is-body approximation. + const SE3 T_cw_last = last_frame_->getPose(); + const SE3 T_wc_last = T_cw_last.inverse(); + const SE3 T_wb_last = T_wc_last * T_cam_imu_; + const Sophus::SO3d R_wb_last = T_wb_last.so3(); + const Vec3 p_wb_last = T_wb_last.translation(); + + // Lever-arm between IMU and camera is small on EuRoC (<= ~5 cm) so we + // treat body and camera world-frame velocities as equal to keep the BA + // prior consumer simple. The residual's loose sigma absorbs the error. + const Vec3 v_wb_last = last_frame_->has_velocity_ + ? last_frame_->velocity_ + : Vec3::Zero(); + const Vec3 gravity_w(0.0, 0.0, -9.81); + + Sophus::SO3d R_wb_new; + Vec3 v_wb_new; + Vec3 p_wb_new; + preint.predict(R_wb_last, v_wb_last, p_wb_last, gravity_w, + R_wb_new, v_wb_new, p_wb_new); + if (!v_wb_new.allFinite()) return; + + current_frame_->velocity_ = v_wb_new; + current_frame_->has_velocity_ = true; + current_frame_->accel_bias_ = last_frame_->accel_bias_; + current_frame_->gyro_bias_ = last_frame_->gyro_bias_; +} + +void Tracking::populateKeyframeImuSpan(const Keyframe::Ptr& kf, + const Keyframe::Ptr& prev_kf) { + if (!kf || !prev_kf || imu_buffer_.empty()) return; + const double t_i = prev_kf->timestamp_; + const double t_j = kf->timestamp_; + if (!(t_j > t_i) || (t_j - t_i) > 5.0) return; + + auto begin_it = std::lower_bound( + imu_buffer_.begin(), imu_buffer_.end(), t_i, + [](const ImuEntry& e, double t) { return e.timestamp_sec < t; }); + auto end_it = std::upper_bound( + imu_buffer_.begin(), imu_buffer_.end(), t_j, + [](double t, const ImuEntry& e) { return t < e.timestamp_sec; }); + if (std::distance(begin_it, end_it) < 2) return; + + ImuPreintegrator preint(prev_kf->accel_bias_, prev_kf->gyro_bias_); + auto prev_it = begin_it; + auto next_it = prev_it + 1; + while (next_it != end_it) { + const double dt_step = next_it->timestamp_sec - prev_it->timestamp_sec; + if (dt_step > 0.0 && dt_step < 0.5) { + preint.integrate(prev_it->accel, prev_it->gyro, dt_step); + } + prev_it = next_it; + ++next_it; + } + if (preint.deltaT() <= 0.0) return; + + auto span = std::make_unique(); + span->delta_R = preint.deltaR(); + span->delta_v = preint.deltaV(); + span->delta_p = preint.deltaP(); + span->dt = preint.deltaT(); + span->bias_accel = prev_kf->accel_bias_; + span->bias_gyro = prev_kf->gyro_bias_; + span->from_kf_id = prev_kf->id_; + span->T_cam_imu = T_cam_imu_; + span->valid = span->delta_v.allFinite() && span->delta_p.allFinite(); + if (span->valid) { + kf->prev_imu_span_ = std::move(span); + } +} + +void Tracking::reconcileVelocityWithVisual() { + if (!last_frame_ || !current_frame_) return; + const double dt = current_frame_->timestamp_ - last_frame_->timestamp_; + if (!(dt > 0.0) || dt > 1.0) return; + + const Vec3 p_wc_curr = current_frame_->getPose().inverse().translation(); + const Vec3 p_wc_last = last_frame_->getPose().inverse().translation(); + const Vec3 v_visual = (p_wc_curr - p_wc_last) / dt; + if (!v_visual.allFinite()) return; + + // IMU prediction weight for the blend. 0 = trust visual fully (default); + // 1 = keep raw IMU estimate untouched. Biases are uncalibrated in Stage + // 0b so a low alpha is usually best. + double alpha_imu = 0.3; + if (const char* env = std::getenv("SVSLAM_VIO_VELOCITY_IMU_ALPHA")) { + char* end = nullptr; + const double parsed = std::strtod(env, &end); + if (end != env && std::isfinite(parsed) && parsed >= 0.0 && parsed <= 1.0) { + alpha_imu = parsed; + } + } + + if (current_frame_->has_velocity_) { + current_frame_->velocity_ = + alpha_imu * current_frame_->velocity_ + (1.0 - alpha_imu) * v_visual; + } else if (std::getenv("SVSLAM_VIO_ENABLE_VISUAL_VELOCITY")) { + // Opt-in: also populate velocity on non-IMU runs so the BA prior acts + // as a trajectory-smoothness regularizer. Default behavior preserves + // existing TUM gates untouched. + current_frame_->velocity_ = v_visual; + current_frame_->has_velocity_ = true; + } +} + +int Tracking::readVioMinInitKeyframes() { + // Env knob to control when Tracking attempts VI init. Default matches + // the ORB-SLAM3 "first ~1–2 s of KFs" heuristic and leaves enough + // dynamics for the linear solve to stay well-conditioned. + int min_kfs = 15; + if (const char* env = std::getenv("SVSLAM_VIO_MIN_INIT_KEYFRAMES")) { + char* end = nullptr; + const long parsed = std::strtol(env, &end, 10); + if (end != env && parsed >= 4 && parsed <= 500) { + min_kfs = static_cast(parsed); + } + } + return min_kfs; +} + +void Tracking::tryVisualInertialInit() { + if (vi_init_done_) return; + if (!map_) return; + if (imu_buffer_.empty()) return; // TUM / non-IMU runs: hard no-op + + const int min_kfs = readVioMinInitKeyframes(); + + // Collect keyframes in temporal order. We intentionally ignore the + // re-init edge cases (their KFs get published in a fresh window) and + // only proceed when the raw KF count has reached the threshold. + std::vector ordered; + { + // Snapshot under the map mutex; the main path already holds it + // briefly elsewhere. Local BA runs on its own thread and may mutate + // kf->T_cw_ during this snapshot, but SE3 copies are safe (trivial + // Eigen types) and we only need a stable *view* for the solve. + const auto& kfs = map_->getAllKeyframes(); + ordered.reserve(kfs.size()); + for (const auto& kv : kfs) { + if (kv.second) ordered.push_back(kv.second); + } + } + std::sort(ordered.begin(), ordered.end(), + [](const Keyframe::Ptr& a, const Keyframe::Ptr& b) { + return a->id_ < b->id_; + }); + + if (static_cast(ordered.size()) < min_kfs) return; + + // The first one or two KFs in a mono session come from the Initializer + // and don't carry a preintegration span (there's no predecessor at + // map bootstrap). Skip them so the window starts from the first KF + // that DOES have a span whose from_kf_id matches its predecessor. + std::size_t window_start = 0; + for (std::size_t i = 1; i < ordered.size(); ++i) { + const auto& span = ordered[i]->prev_imu_span_; + if (!span || !span->valid) continue; + if (span->from_kf_id != ordered[i - 1]->id_) continue; + window_start = i - 1; + break; + } + + if (window_start + static_cast(min_kfs) > ordered.size()) { + // not yet enough usable KFs + return; + } + + std::vector window(ordered.begin() + window_start, + ordered.begin() + window_start + min_kfs); + + // Every KF past the first one in the window must have a valid span + // linked to its immediate predecessor; skip the attempt otherwise. + for (std::size_t i = 1; i < window.size(); ++i) { + const auto& span = window[i]->prev_imu_span_; + if (!span || !span->valid) return; + if (span->from_kf_id != window[i - 1]->id_) return; + } + ordered = std::move(window); + + ++vi_init_attempts_; + VisualInertialInitializer::Options opts; + // EuRoC mono: initializer ran a 1 / median_depth rescale during + // bootstrap, so the map scale is unitless. RGB-D / stereo runs would + // already be metric and should set metric_scale=true — but for the + // current Stage 0c.e scope only the mono+IMU path reaches here. + opts.metric_scale = false; + // The existing accel-alignment at init time doesn't set the world + // frame up strictly-z-up (see the bug noted in + // visual_inertial_initializer.cc): let the LSQ solve for gravity in + // the post-init world frame, then we'll rotate so gravity ends up + // along world -Z. + opts.assume_gravity_w = Vec3::Zero(); + VisualInertialInitializer vi(opts); + const auto result = vi.initialize(ordered); + + if (!result.converged) { + if (vi_init_attempts_ <= 3 || + (vi_init_attempts_ % 5) == 0) { + std::cout << "Tracking: VI init not converged (attempt " + << vi_init_attempts_ << "): " << result.message + << " scale=" << result.scale + << " rot_rms=" << result.rotation_residual_rms + << " lin_rms=" << result.linear_residual_rms + << std::endl; + } + return; + } + + // Compute the 3×3 rotation R_wn_w that maps the current visual world + // frame into the VI-calibrated world frame (gravity → (0,0,-9.81)). + // Using the cross-product form keeps it numerically stable when the + // two vectors are close to parallel. + const Vec3 g_target(0.0, 0.0, -opts.gravity_magnitude); + const Vec3 g_est_unit = result.gravity_w.normalized(); + const Vec3 g_tgt_unit = g_target.normalized(); + const Vec3 cross = g_est_unit.cross(g_tgt_unit); + const double dot = g_est_unit.dot(g_tgt_unit); + Eigen::Matrix3d R_wn_w = Eigen::Matrix3d::Identity(); + if (cross.norm() > 1e-8) { + Eigen::Matrix3d K; + K << 0.0, -cross.z(), cross.y(), + cross.z(), 0.0, -cross.x(), + -cross.y(), cross.x(), 0.0; + const double s = cross.norm(); + R_wn_w = Eigen::Matrix3d::Identity() + K + K * K * ((1.0 - dot) / (s * s)); + } else if (dot < 0.0) { + // Anti-parallel: flip about any axis orthogonal to g_est_unit. + Eigen::Vector3d axis = g_est_unit.unitOrthogonal(); + R_wn_w = Eigen::AngleAxisd(M_PI, axis).toRotationMatrix(); + } + + const Sophus::SO3d R_wn_w_so3(Eigen::Quaterniond(R_wn_w).normalized()); + const SE3 T_wn_w(R_wn_w_so3, Vec3::Zero()); + + const double s = result.scale; + if (!(std::isfinite(s) && s > 0.0)) { + std::cout << "Tracking: VI init returned invalid scale, skipping" << std::endl; + return; + } + + // Rescale + rotate every KF pose and every landmark in the map. + // We rewrite the entire map (local BA may have already injected + // corrections for early KFs, but rotate+scale commutes with BA — we + // just apply the transform consistently). This block must run BEFORE + // the preintegration residual is enabled so the gravity vector we + // plug into VelocityPreintegrationError matches the new frame. + // + // Re-use the loop_correcting_ flag so the LocalMapping thread skips + // its BA step while we rewrite the map. The flag is checked inside + // LocalMapping::processPendingWork() (see backend/local_mapping.cc). + map_->loop_correcting_.store(true); + { + std::lock_guard map_lock(map_->mutex_); + for (const auto& kv : map_->getAllKeyframes()) { + auto& kf = kv.second; + if (!kf) continue; + // T_cw' = T_cw * T_w_wn with w_wn = (T_wn_w * diag(s))^{-1}. + // For a uniform rescale + rotation in world, the closed-form + // update for a camera pose T_cw (world→cam) is: + // p_wc' = s * R_wn_w * p_wc + // R_cw' = R_cw * R_wn_w^T + // which gives a new T_cw' = [R_cw', -R_cw' * p_wc']. + const SE3 T_wc = kf->T_cw_.inverse(); + const Vec3 p_wc = T_wc.translation(); + const Sophus::SO3d R_wc = T_wc.so3(); + const Vec3 p_wc_new = s * (R_wn_w_so3 * p_wc); + const Sophus::SO3d R_wc_new = R_wn_w_so3 * R_wc; + const SE3 T_wc_new(R_wc_new, p_wc_new); + kf->T_cw_ = T_wc_new.inverse(); + } + for (const auto& kv : map_->getAllLandmarks()) { + auto& lm = kv.second; + if (!lm) continue; + const Vec3 p = lm->getPos(); + if (!p.allFinite()) continue; + lm->setPos(s * (R_wn_w_so3 * p)); + } + } + map_->loop_correcting_.store(false); + + // Also fold the transform into current/last frames so the next call + // to track() uses pose references consistent with the re-scaled map. + auto rotate_frame_state = [&](Frame::Ptr& frame) { + if (!frame) return; + const SE3 T_wc = frame->getPose().inverse(); + const SE3 T_wc_new(R_wn_w_so3 * T_wc.so3(), s * (R_wn_w_so3 * T_wc.translation())); + frame->setPose(T_wc_new.inverse()); + if (frame->has_velocity_ && frame->velocity_.allFinite()) { + frame->velocity_ = R_wn_w_so3 * frame->velocity_; + } + }; + rotate_frame_state(current_frame_); + if (last_frame_ && last_frame_ != current_frame_) { + rotate_frame_state(last_frame_); + } + + // Motion model translation also scales. Rotation component rotates by + // R_wn_w (a similarity transform on the velocity SE3). + { + const Sophus::SO3d R_new = R_wn_w_so3 * velocity_.so3() * R_wn_w_so3.inverse(); + const Vec3 t_new = s * (R_wn_w_so3 * velocity_.translation()); + velocity_ = SE3(R_new, t_new); + } + + // Write biases + velocities into the window KFs. Velocities rotate + // into the new world frame (they do not scale — already in m/s). + for (std::size_t i = 0; i < ordered.size(); ++i) { + auto& kf = ordered[i]; + if (!kf) continue; + const Vec3 v_new = R_wn_w_so3 * result.velocities[i]; + if (v_new.allFinite()) { + kf->velocity_ = v_new; + kf->has_velocity_ = true; + } + kf->accel_bias_ = result.accel_bias; + kf->gyro_bias_ = result.gyro_bias; + } + + // Propagate the newly-estimated gyro bias into every span's delta_R + // via the first-order Forster correction. Without this, the BA + // rotation residual (Stage 0c.d) keeps enforcing the uncorrected + // delta_R path while poses adopt the new bias, and the mismatch + // amplifies the ATE — exactly the combined regression we measured + // on MH_01 (1.41 → 2.28 m) before this fix landed. + vi.applyGyroBiasCorrectionToSpans(ordered, result); + + // For KFs beyond the init window we leave biases and velocities at + // their last values — the BA random-walk prior and reconcileVelocityWithVisual + // will pull them in quickly now that gravity + scale are metric. + + vi_init_done_ = true; + vi_init_scale_ = s; + vi_init_gravity_w_ = g_target; // canonical after rotation + gravity_aligned_ = true; // future KFs skip the ad-hoc accel alignment + + // Unblock the IMU preintegration residual in local BA. The loose + // velocity prior is kept as a fallback for pairs without a valid span. + Optimizer::setPreintegrationResidualEnabled(true); + + std::cout << "Tracking: VI init SUCCESS (scale=" << s + << ", rot_rms=" << result.rotation_residual_rms + << ", lin_rms=" << result.linear_residual_rms + << ", |g|=" << opts.gravity_magnitude + << ", gyro_bias=[" << result.gyro_bias.transpose() << "]" + << ", window=" << ordered.size() << " KFs)" + << std::endl; +} + } diff --git a/src/tracking/tracking.h b/src/tracking/tracking.h index e4bff39..452cbbe 100644 --- a/src/tracking/tracking.h +++ b/src/tracking/tracking.h @@ -10,8 +10,10 @@ #include "core/map.h" #include "core/reference_keyframe_policy.h" #include "tracking/initializer.h" +#include "tracking/visual_inertial_initializer.h" #include "backend/local_mapping.h" #include "io/tum_dataset.h" +#include "sensors/imu.h" #include #include @@ -89,6 +91,30 @@ class Tracking { std::vector accel_buffer_; bool gravity_aligned_ = false; + // Full IMU (accel + gyro) buffer — populated when the dataset exposes + // imu0 (EuRoC). Stage 0b scaffolding: tracking still consults + // accel_buffer_ for gravity / stationary detection; imu_buffer_ is held + // here so a future VIO path (preintegration, velocity predict) can read + // it without threading more state through the call sites. + std::vector imu_buffer_; + + // IMU→camera extrinsic (T_cam_imu: transforms IMU/body frame points into + // the camera frame). Defaults to identity for datasets without a known + // extrinsic (TUM, or EuRoC without sensor.yaml). Set from EurocDataset + // in run_mono before the first frame arrives. + SE3 T_cam_imu_; + bool has_cam_imu_extrinsic_ = false; + + void setImuToCameraExtrinsic(const SE3& T_cam_imu) { + T_cam_imu_ = T_cam_imu; + has_cam_imu_extrinsic_ = true; + } + + // Snapshot of the VI init state for monitoring / automation. + bool visualInertialInitCompleted() const { return vi_init_done_; } + double visualInertialInitScale() const { return vi_init_scale_; } + const Vec3& visualInertialInitGravity() const { return vi_init_gravity_w_; } + private: struct RecoveryState { int lost_frame_count = 0; @@ -123,6 +149,25 @@ class Tracking { bool reinitialize(); // Re-initialize from scratch when lost for too long void setReferenceKeyframe(Keyframe::Ptr kf); void setKeyframeGravity(Keyframe::Ptr kf); // Set gravity from accel data + // Preintegrate imu_buffer_ between last_frame_ and current_frame_ and + // write an IMU-predicted world-frame velocity into current_frame_. + // No-op unless gravity_aligned_ and IMU samples span the interval. + void predictVelocityFromImu(); + // Fold the post-tracking visual pose delta into current_frame_->velocity_ + // so Keyframe::velocity_ (and therefore the BA velocity prior) reflects a + // visually-corrected IMU estimate instead of pure open-loop integration. + void reconcileVelocityWithVisual(); + // Preintegrate imu_buffer_ from prev_kf's timestamp to kf's timestamp and + // attach the resulting span to kf->prev_imu_span_ for BA consumption. + void populateKeyframeImuSpan(const std::shared_ptr& kf, + const std::shared_ptr& prev_kf); + // Try to bootstrap the VIO estimate (scale, gravity, biases, velocities) + // by running VisualInertialInitializer on the first N KFs. On success + // the map is re-scaled + rotated so gravity aligns with world Z-up, + // biases and velocities are written back to the KFs, and + // vi_init_done_ flips to true so the BA preintegration residual is + // unblocked. No-op on TUM / datasets without an IMU stream. + void tryVisualInertialInit(); static std::size_t countValidFrameLandmarks(const Frame::Ptr& frame); cv::Ptr matcher_; @@ -147,6 +192,17 @@ class Tracking { static constexpr int reinit_trigger_frames_ = 20; // Start re-init after this many lost frames TrackingRunStatistics run_stats_; + + // Visual-Inertial Initialization (VIO Stage 0c.e). Once complete, the + // map is in metric scale with gravity along world -Z, per-KF biases + + // velocities are seeded, and downstream BA can tightly couple the + // preintegration residual. Before completion, BA should suppress the + // preintegration residual and fall back to the loose velocity prior. + bool vi_init_done_ = false; + int vi_init_attempts_ = 0; + double vi_init_scale_ = 1.0; + Vec3 vi_init_gravity_w_ = Vec3(0.0, 0.0, -9.81); + static int readVioMinInitKeyframes(); }; } diff --git a/src/tracking/visual_inertial_initializer.cc b/src/tracking/visual_inertial_initializer.cc new file mode 100644 index 0000000..3977f92 --- /dev/null +++ b/src/tracking/visual_inertial_initializer.cc @@ -0,0 +1,403 @@ +#include "tracking/visual_inertial_initializer.h" + +#include +#include + +#include +#include + +namespace svslam { + +namespace { + +// Small helper to make a VI-init Result carrying an error message and +// leaving the rest of the fields at their defaults. +VisualInertialInitializer::Result makeFailure(const std::string& msg) { + VisualInertialInitializer::Result r; + r.converged = false; + r.message = msg; + return r; +} + +// Visual rotation R_wb = R_wc * R_cb, computed from a KF's current pose +// T_cw and the camera-from-IMU extrinsic stored with the span. +Eigen::Matrix3d bodyRotationInWorld(const Keyframe::Ptr& kf, + const Sophus::SO3d& R_cb) { + // T_cw_ maps world → camera; inverse gives world-from-camera pose. + const Sophus::SO3d R_wc = kf->T_cw_.so3().inverse(); + return (R_wc * R_cb).matrix(); +} + +// Right-Jacobian inverse times a small rotation vector — used for +// approximating the first-order gyro-bias Jacobian of delta_R. +// J_R^{-1}(phi) ≈ I + 0.5 * [phi]_x for small phi; for small bias updates +// it's accurate enough to fold into the closed-form bias solve. For our +// purposes we just assume J ≈ -I * dt, which is the textbook result when +// the rotation change is dominated by the bias term (see Forster et al.). + +} // namespace + +VisualInertialInitializer::Result VisualInertialInitializer::initialize( + const std::vector& keyframes) const { + if (keyframes.size() < 3) { + return makeFailure("need at least 3 keyframes"); + } + + // Basic sanity + topology checks. Every KF past the first one must + // carry a valid span whose `from_kf_id` points at the preceding KF. + // We pull R_cb / t_cb from the first valid span so the rest of the + // algorithm can use a single extrinsic; the initializer is not + // designed to handle per-KF extrinsic variation. + Sophus::SO3d R_cb_global; + Eigen::Vector3d t_cb_global = Eigen::Vector3d::Zero(); + bool extrinsic_ready = false; + + for (std::size_t i = 1; i < keyframes.size(); ++i) { + const auto& kf_i = keyframes[i - 1]; + const auto& kf_j = keyframes[i]; + if (!kf_i || !kf_j) { + return makeFailure("null keyframe in window"); + } + if (!kf_j->prev_imu_span_ || !kf_j->prev_imu_span_->valid) { + return makeFailure("missing / invalid prev_imu_span_"); + } + if (kf_j->prev_imu_span_->from_kf_id != kf_i->id_) { + return makeFailure("span predecessor mismatch"); + } + if (!(kf_j->prev_imu_span_->dt > 0.0)) { + return makeFailure("non-positive span dt"); + } + if (!extrinsic_ready) { + R_cb_global = kf_j->prev_imu_span_->T_cam_imu.so3(); + t_cb_global = kf_j->prev_imu_span_->T_cam_imu.translation(); + extrinsic_ready = true; + } + } + + if (!extrinsic_ready) { + return makeFailure("no extrinsic available"); + } + + // Cumulative duration guard — short windows leave scale / gravity + // poorly observable since the 0.5*g*dt^2 term dominates the linear + // system only when dt is not microscopic. + double total_dt = 0.0; + for (std::size_t i = 1; i < keyframes.size(); ++i) { + total_dt += keyframes[i]->prev_imu_span_->dt; + } + if (total_dt < options_.min_total_dt_seconds) { + return makeFailure("insufficient cumulative span duration"); + } + + // ---------- Stage 1: closed-form gyro bias ---------- + // + // Preintegration gave us delta_R at bias_gyro = span.bias_gyro. The + // visually observed rotation between KF_i and KF_j, expressed in the + // body frame, is: dR_obs = R_wb_i^T * R_wb_j. + // Approximating the bias-update Jacobian as -I * dt, the residual + // r_i = Log((delta_R)^T * dR_obs) ≈ -dbg * dt, stacked across all + // pairs gives a simple diagonal-weighted 3×3 system. + Eigen::Vector3d estimated_gyro_bias = keyframes[1]->prev_imu_span_->bias_gyro; + double rotation_residual_rms = 0.0; + { + Eigen::Matrix3d H = Eigen::Matrix3d::Zero(); + Eigen::Vector3d rhs = Eigen::Vector3d::Zero(); + double residual_sum_sq = 0.0; + int pair_count = 0; + for (std::size_t i = 1; i < keyframes.size(); ++i) { + const auto& span = *keyframes[i]->prev_imu_span_; + const Eigen::Matrix3d R_wb_i = bodyRotationInWorld(keyframes[i - 1], R_cb_global); + const Eigen::Matrix3d R_wb_j = bodyRotationInWorld(keyframes[i], R_cb_global); + const Eigen::Matrix3d dR_obs = R_wb_i.transpose() * R_wb_j; + const Eigen::Matrix3d R_err = span.delta_R.matrix().transpose() * dR_obs; + const Eigen::Vector3d r = Sophus::SO3d(Eigen::Quaterniond(R_err)).log(); + // J = -I * dt (first order); so dbg satisfies J * dbg = r + // => dbg = -r / dt; stacked: H = sum(dt^2 * I), rhs = sum(dt * -r) + // Equivalent to H * dbg = rhs with H = sum(dt^2) * I. + const double dt = span.dt; + H += dt * dt * Eigen::Matrix3d::Identity(); + rhs += -dt * r; + residual_sum_sq += r.squaredNorm(); + ++pair_count; + } + if (pair_count == 0) { + return makeFailure("gyro bias system degenerate"); + } + // Tikhonov regularization toward zero. Short EuRoC windows leave + // the bias underdetermined, so an unregularized solve blows up + // (observed |bg| ~ 0.15 rad/s against a true ~0.004 rad/s). The + // prior pulls weakly toward zero while still letting real biases + // show through. + if (options_.gyro_bias_prior_sigma > 0.0) { + const double lambda = 1.0 / + (options_.gyro_bias_prior_sigma * options_.gyro_bias_prior_sigma); + H += lambda * Eigen::Matrix3d::Identity(); + // rhs += lambda * 0 (zero prior mean). + } + if (H.determinant() < 1e-18) { + return makeFailure("gyro bias system degenerate"); + } + Eigen::Vector3d dbg = H.ldlt().solve(rhs); + if (!dbg.allFinite()) { + return makeFailure("gyro bias solve non-finite"); + } + // Hard cap — if the regularized solve still lands outside a sane + // range, fall back to zero. Better to let the BA bias blocks + // discover the true bias than to seed preintegration with a wildly + // wrong delta_R correction. + if (options_.gyro_bias_magnitude_cap > 0.0 && + dbg.cwiseAbs().maxCoeff() > options_.gyro_bias_magnitude_cap) { + dbg.setZero(); + } + estimated_gyro_bias = keyframes[1]->prev_imu_span_->bias_gyro + dbg; + rotation_residual_rms = + pair_count > 0 ? std::sqrt(residual_sum_sq / pair_count) : 0.0; + } + + // ---------- Stage 2: scale + gravity + per-KF velocities ---------- + // + // For each pair (i, j), use the Forster preintegration equations with + // accel_bias fixed at span.bias_accel (kept as reference — the linear + // fit can't tease scale + bias apart in a short window, so we leave + // bias refinement to the BA that consumes this bootstrap): + // + // s * (p_wb_j - p_wb_i) = v_i * dt_ij + 0.5 * g_w * dt_ij^2 + // + R_wb_i * delta_p + // + // v_j - v_i = g_w * dt_ij + R_wb_i * delta_v + // + // Unknowns: x = [v_0 | v_1 | ... | v_{N-1} | g_w | s] + // size = 3*N + 3 + 1 (when `metric_scale` is false) + // size = 3*N + 3 (when `metric_scale` is true) + const std::size_t N = keyframes.size(); + const bool solve_scale = !options_.metric_scale; + const bool gravity_fixed = options_.assume_gravity_w.allFinite() && + options_.assume_gravity_w.norm() > 1e-6; + const Eigen::Vector3d g_fixed = + gravity_fixed ? options_.assume_gravity_w : Eigen::Vector3d::Zero(); + const std::size_t vel_dim = 3 * N; + const std::size_t grav_off = vel_dim; // only used when !gravity_fixed + const std::size_t scale_off = gravity_fixed ? vel_dim : (vel_dim + 3); + const std::size_t unknowns = + vel_dim + (gravity_fixed ? 0 : 3) + (solve_scale ? 1 : 0); + const bool add_scale_prior = solve_scale && options_.scale_prior_weight > 0.0; + const std::size_t rows = 6 * (N - 1) + (add_scale_prior ? 1 : 0); + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(rows, unknowns); + Eigen::VectorXd b = Eigen::VectorXd::Zero(rows); + + // The visual mono rescaling applies to *camera* positions (that's what + // median-depth normalisation touches), not to the IMU lever-arm t_bc. + // Using camera-frame positions in the LSQ avoids coupling scale into + // the lever-arm term: + // p_wc_metric = p_wb_metric + R_wb * t_bc + // ⇒ p_wb_metric_j - p_wb_metric_i + // = (p_wc_metric_j - p_wc_metric_i) - (R_wb_j - R_wb_i) * t_bc + // = s * (p_wc_visual_j - p_wc_visual_i) - (R_wb_j - R_wb_i) * t_bc + // Substituting into the body-frame Forster equation and re-arranging, + // the scale coefficient is cleanly (p_wc_j - p_wc_i) only. + const Eigen::Vector3d t_bc = -(R_cb_global.inverse() * t_cb_global); + + for (std::size_t i = 1; i < N; ++i) { + const auto& span = *keyframes[i]->prev_imu_span_; + const double dt = span.dt; + const double half_dt2 = 0.5 * dt * dt; + + const Eigen::Matrix3d R_wb_i = bodyRotationInWorld(keyframes[i - 1], R_cb_global); + const Eigen::Matrix3d R_wb_j = bodyRotationInWorld(keyframes[i], R_cb_global); + + // Camera-in-world positions (visual frame — these are what the + // downstream rescale multiplies by s). + const Eigen::Vector3d p_wc_i = keyframes[i - 1]->T_cw_.inverse().translation(); + const Eigen::Vector3d p_wc_j = keyframes[i]->T_cw_.inverse().translation(); + + const Eigen::Vector3d R_dp = R_wb_i * span.delta_p; + const Eigen::Vector3d R_dv = R_wb_i * span.delta_v; + const Eigen::Vector3d lever_term = (R_wb_j - R_wb_i) * t_bc; + + const std::size_t row_pos = 6 * (i - 1); + const std::size_t row_vel = row_pos + 3; + + // Position: s*(p_wc_j - p_wc_i) - dt*v_i - 0.5*dt^2*g + // = R_wb_i*delta_p + (R_wb_j - R_wb_i) * t_bc + A.block<3, 3>(row_pos, 3 * (i - 1)) = -dt * Eigen::Matrix3d::Identity(); // v_i + Eigen::Vector3d rhs_pos = R_dp + lever_term; + if (gravity_fixed) { + // Bake the fixed gravity term into the RHS. + rhs_pos += half_dt2 * g_fixed; + } else { + A.block<3, 3>(row_pos, grav_off) = -half_dt2 * Eigen::Matrix3d::Identity(); // g + } + if (solve_scale) { + A.block<3, 1>(row_pos, scale_off) = p_wc_j - p_wc_i; // s + b.segment<3>(row_pos) = rhs_pos; + } else { + b.segment<3>(row_pos) = rhs_pos - (p_wc_j - p_wc_i); + } + + // Velocity: v_j - v_i - dt*g = R_wb_i*delta_v (no scale coupling) + A.block<3, 3>(row_vel, 3 * i) = Eigen::Matrix3d::Identity(); // v_j + A.block<3, 3>(row_vel, 3 * (i - 1)) -= Eigen::Matrix3d::Identity(); // -v_i + Eigen::Vector3d rhs_vel = R_dv; + if (gravity_fixed) { + rhs_vel += dt * g_fixed; + } else { + A.block<3, 3>(row_vel, grav_off) = -dt * Eigen::Matrix3d::Identity(); + } + b.segment<3>(row_vel) = rhs_vel; + } + + if (add_scale_prior) { + const std::size_t row = 6 * (N - 1); + A(row, scale_off) = options_.scale_prior_weight; + b(row) = options_.scale_prior_weight * options_.scale_prior; + } + + // Least squares solve. Bounded scale check afterwards. + Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); + const Eigen::VectorXd x = svd.solve(b); + if (!x.allFinite()) { + return makeFailure("linear solve non-finite"); + } + + if (std::getenv("SVSLAM_VIO_INIT_DEBUG")) { + std::cerr << "[VI init debug] N=" << N << " unknowns=" << unknowns + << " rows=" << rows + << " gravity_fixed=" << gravity_fixed + << " solve_scale=" << solve_scale << std::endl; + std::cerr << " T_cb (cam from imu) R=\n" << R_cb_global.matrix() + << "\n t_cb=" << t_cb_global.transpose() << std::endl; + std::cerr << " KF IDs: "; + for (std::size_t i = 0; i < std::min(N, 15); ++i) { + std::cerr << keyframes[i]->id_ << " "; + } + std::cerr << std::endl; + for (std::size_t i = 1; i < std::min(N, 4); ++i) { + const auto& span = *keyframes[i]->prev_imu_span_; + const Eigen::Vector3d p_wc_i = keyframes[i - 1]->T_cw_.inverse().translation(); + const Eigen::Vector3d p_wc_j = keyframes[i]->T_cw_.inverse().translation(); + const Eigen::Matrix3d R_wb_i = bodyRotationInWorld(keyframes[i - 1], R_cb_global); + const Eigen::Matrix3d R_wc_i = keyframes[i - 1]->T_cw_.inverse().so3().matrix(); + const Eigen::Vector3d R_dp = R_wb_i * span.delta_p; + const Eigen::Vector3d g_in_body_est = R_wb_i.transpose() * Eigen::Vector3d(0, 0, -9.81); + std::cerr << " pair " << (i - 1) << "->" << i + << " dt=" << span.dt + << "\n dp_wc_visual=" << (p_wc_j - p_wc_i).transpose() + << "\n R_dp_metric =" << R_dp.transpose() + << "\n dp_body =" << span.delta_p.transpose() + << "\n g_in_body =" << g_in_body_est.transpose() + << "\n R_wc_i.det =" << R_wc_i.determinant() + << std::endl; + } + Eigen::Vector3d g_sol = gravity_fixed ? g_fixed + : Eigen::Vector3d(x[grav_off], x[grav_off+1], x[grav_off+2]); + std::cerr << " solution scale=" << (solve_scale ? x[scale_off] : 1.0) + << "\n velocity_0=" << Eigen::Vector3d(x[0], x[1], x[2]).transpose() + << "\n gravity_world=" << g_sol.transpose() + << " (|g|=" << g_sol.norm() << ")" + << std::endl; + } + + const Eigen::VectorXd residual = A * x - b; + const double linear_rms = std::sqrt(residual.squaredNorm() / std::max(1, rows)); + + Result out; + out.velocities.reserve(N); + for (std::size_t i = 0; i < N; ++i) { + out.velocities.emplace_back(x[3 * i], x[3 * i + 1], x[3 * i + 2]); + } + out.gravity_w = gravity_fixed + ? g_fixed + : Eigen::Vector3d(x[grav_off], x[grav_off + 1], x[grav_off + 2]); + out.scale = solve_scale ? x[scale_off] : 1.0; + out.gyro_bias = estimated_gyro_bias; + out.accel_bias = Eigen::Vector3d::Zero(); // first-pass pin + out.rotation_residual_rms = rotation_residual_rms; + out.linear_residual_rms = linear_rms; + + // Sanity gates. + if (solve_scale) { + const double s = out.scale; + const double ratio = s > 0.0 ? s : -s; + if (!(s > 0.0)) { + out.message = "non-positive scale"; + out.converged = false; + return out; + } + if (ratio > options_.max_scale_ratio_deviation || + 1.0 / ratio > options_.max_scale_ratio_deviation) { + out.message = "scale outside tolerance"; + out.converged = false; + return out; + } + } + + const double g_norm = out.gravity_w.norm(); + if (!(g_norm > 1.0)) { + out.message = "gravity vector near zero"; + out.converged = false; + return out; + } + + // Rescale gravity to the expected magnitude. This preserves the + // direction while letting the downstream BA math use |g| = 9.81 + // without tracking a per-run scale. + out.gravity_w *= (options_.gravity_magnitude / g_norm); + + // Propagate the scale into the velocities so the caller can consume a + // coherent (metric) trajectory. (p_j - p_i) * s accounts for scale in + // positions, but v_i was solved in metric units directly — no + // rescaling needed. Scale affects positions only; velocities are + // already in m/s. + + if (!std::isfinite(linear_rms) || linear_rms > options_.linear_residual_rms_max) { + out.message = "linear residual too large"; + out.converged = false; + return out; + } + + // Final convergence gate — rotation residual failure also prevents + // success. + if (rotation_residual_rms > options_.rotation_residual_rms_max) { + out.message = "rotation residual too large"; + out.converged = false; + return out; + } + + out.converged = true; + if (out.message.empty()) { + std::ostringstream os; + os << "VI init OK (scale=" << out.scale + << " |g|=" << options_.gravity_magnitude + << " rot_rms=" << out.rotation_residual_rms + << " lin_rms=" << out.linear_residual_rms + << ")"; + out.message = os.str(); + } + return out; +} + +void VisualInertialInitializer::applyGyroBiasCorrectionToSpans( + const std::vector& keyframes, + const Result& result) const { + if (!result.converged) return; + if (keyframes.size() < 2) return; + + for (std::size_t i = 1; i < keyframes.size(); ++i) { + auto& kf = keyframes[i]; + if (!kf || !kf->prev_imu_span_ || !kf->prev_imu_span_->valid) continue; + auto& span = *kf->prev_imu_span_; + + // First-order Forster correction: delta_R(bg) ≈ delta_R_ref * + // Exp(-J_r * (bg_new - bg_ref) * dt). For short inter-KF intervals + // we approximate J_r = I, which keeps the update a 3-vector tangent + // rotation that's safe to compose with Sophus::SO3d. + const Eigen::Vector3d dbg = result.gyro_bias - span.bias_gyro; + if (!dbg.allFinite()) continue; + const Eigen::Vector3d phi = -dbg * span.dt; + span.delta_R = span.delta_R * Sophus::SO3d::exp(phi); + span.bias_gyro = result.gyro_bias; + } +} + +} // namespace svslam diff --git a/src/tracking/visual_inertial_initializer.h b/src/tracking/visual_inertial_initializer.h new file mode 100644 index 0000000..217ef75 --- /dev/null +++ b/src/tracking/visual_inertial_initializer.h @@ -0,0 +1,164 @@ +#pragma once + +#include +#include + +#include "core/common.h" +#include "core/keyframe.h" + +namespace svslam { + +// Visual-Inertial Initializer (VIO Stage 0c.e). +// +// Given a window of recent keyframes carrying frozen preintegration spans +// from their immediate predecessors (see `ImuPreintegrationSpan`), this +// class estimates a bootstrap VIO state: +// +// - scale (mono runs only — set to 1 on stereo / metric-depth), +// - gravity direction in the current visual-map world frame, +// - initial accel + gyro biases, +// - per-keyframe world-frame velocities. +// +// The algorithm is intentionally simple (loosely following the two-stage +// "inertial-only optimization" used by ORB-SLAM3 Appendix A): +// +// 1. Closed-form gyro bias from `delta_R` vs the visually observed +// rotation between consecutive KFs, solved as a 3×3 LSQ using the +// first-order Jacobian J_dR_dbg ≈ -I · dt. +// 2. Linear least squares for {scale, gravity_world, v_0…v_{N-1}} from +// the position + velocity preintegration equations, with accel_bias +// pinned to zero for this first pass (the local BA's accel-bias +// parameter + BiasRandomWalkError then take it from there). The +// resulting gravity vector is rescaled to 9.81 m/s^2 after the solve. +// +// The initializer never touches the map directly; the caller (`Tracking`) +// is responsible for re-scaling KF poses + landmarks and rotating the map +// so the estimated gravity aligns with world Z-up. +class VisualInertialInitializer { +public: + struct Result { + // True only if the LSQ solves produced finite values and the + // residual norms are within tolerance. Callers should fall back + // to the pre-init behaviour otherwise. + bool converged = false; + + // Estimated map scale (mono): multiply KF translations + landmark + // positions by `scale` to convert to metric units. 1.0 for stereo. + double scale = 1.0; + + // Gravity vector in the current (pre-rotation) visual world frame, + // expressed in m/s^2. After the caller rotates the map so this + // vector aligns with (0, 0, -9.81), downstream code can use + // gravity_w = Vec3(0,0,-9.81) straight from the VelocityPreintegrationError. + Vec3 gravity_w = Vec3(0.0, 0.0, -9.81); + + // Accel / gyro biases recovered from the window. accel_bias is + // pinned to zero in the current first-pass implementation. + Vec3 accel_bias = Vec3::Zero(); + Vec3 gyro_bias = Vec3::Zero(); + + // Per-KF velocities in the (pre-rotation) visual world frame. + // Size matches the input keyframe window. + std::vector velocities; + + // Residual norms of the two LSQ stages (for logging / debugging). + double rotation_residual_rms = 0.0; + double linear_residual_rms = 0.0; + + // Optional human-readable message populated on failure. + std::string message; + }; + + struct Options { + // When true, treat the visual map as already metric-scale and + // clamp `scale` to 1.0 in the linear stage. Set this for stereo + // / RGB-D datasets. Mono init should leave it false. + bool metric_scale = false; + + // Maximum absolute deviation of the estimated scale from 1.0 we + // will accept (mono only). Helps reject degenerate windows where + // the IMU sees no translation excitation. + double max_scale_ratio_deviation = 10.0; + + // Gravity magnitude we expect at the surface. Used to re-normalize + // the estimated gravity vector so downstream BA code can assume + // |g| = 9.81. + double gravity_magnitude = 9.81; + + // When `assume_gravity_w` is finite, the linear stage treats + // gravity as a fixed vector (the caller supplies the accel-aligned + // (0, 0, -9.81) world-frame vector) and solves only for + // {scale, per-KF velocities}. This breaks the scale/gravity sign + // ambiguity when the visual map is already gravity-aligned at + // initialization time (the EuRoC mono path). + // + // Set to Vec3::Zero() to enable the full 3-DoF gravity solve. + Vec3 assume_gravity_w = Vec3(0.0, 0.0, -9.81); + + // Soft prior pulling the estimated scale toward `scale_prior` + // (units: map-visual per metric). Heuristic: if mono init ran a + // 1/median_depth rescale with median ≈ 1–10 m, the recovered + // scale should be in that range. The prior breaks the small sign + // ambiguity that can arise when the visual world frame is + // slightly rotated relative to the "true" gravity-aligned frame + // and the linear LSQ finds two near-equivalent fits. + double scale_prior = 1.0; + double scale_prior_weight = 1.0; // 0 disables + + // Residual norm thresholds above which we declare the solve + // unreliable. Tightened rotation threshold (0.08 rad ≈ 4.6°) + // rejects windows where the visual rotations diverge from the + // IMU-integrated delta_R by more than bias alone can explain — + // otherwise the BA rotation residual (Stage 0c.d) fights the + // un-reconciled mismatch and the trajectory degrades. Observed + // rot_rms: 0.13 rad on MH_01's first 15 mono KFs (rejected), + // 0.05 rad on V1_01's (accepted). + double rotation_residual_rms_max = 0.08; // rad per pair + double linear_residual_rms_max = 3.0; // m / (m/s) per pair + + // Minimum cumulative preintegration duration across the window. + // Short windows (≈0.5 s) leave scale/gravity poorly observable. + double min_total_dt_seconds = 1.0; + + // Tikhonov regularization on the gyro-bias LSQ: information matrix + // gets (1 / sigma^2) added on its diagonal. Short windows leave the + // bias direction underdetermined, so unregularized solves can blow + // up. Default is 0 (disabled) because with EuRoC's typical per-KF + // dt the regularization strong enough to suppress blow-up also + // kills the real signal; the cap below is the primary guardrail. + // Set to a positive value to enable Tikhonov damping. + double gyro_bias_prior_sigma = 0.0; + + // Hard cap on the estimated gyro-bias magnitude. If the + // unregularized solve lands outside this box, the bias update is + // discarded (falls back to zero) and the BA bias blocks pick up + // the slack. Measured EuRoC biases are O(0.01 rad/s); the cap + // catches the O(0.1+) over-fit we observed on MH_01 / V1_01. + double gyro_bias_magnitude_cap = 0.05; // rad/s + }; + + VisualInertialInitializer() = default; + explicit VisualInertialInitializer(const Options& options) : options_(options) {} + + const Options& options() const { return options_; } + + // Run the two-stage solve over a monotonically-ordered (by time) window + // of keyframes. Each KF (except the first) must carry a valid + // `prev_imu_span_` whose `from_kf_id` matches the predecessor. + Result initialize(const std::vector& keyframes) const; + + // Apply the first-order gyro-bias correction to the spans attached to + // `keyframes`. After Result::converged, callers should invoke this so + // the BA's rotation residual sees delta_R values consistent with the + // newly-estimated bias. Uses the short-interval first-order Jacobian + // J_r ≈ I and updates `span.bias_gyro` so future BA bias updates are + // measured relative to the corrected reference. + void applyGyroBiasCorrectionToSpans( + const std::vector& keyframes, + const Result& result) const; + +private: + Options options_; +}; + +} // namespace svslam diff --git a/tests/test_euroc_dataset.cc b/tests/test_euroc_dataset.cc index 3cc2cec..40384e4 100644 --- a/tests/test_euroc_dataset.cc +++ b/tests/test_euroc_dataset.cc @@ -1,9 +1,12 @@ #include +#include +#include #include #include #include #include +#include #include @@ -14,9 +17,21 @@ using namespace svslam; namespace { +// Build a temp path that is unique across concurrent ctest workers. Previously +// relied on std::rand() (deterministic, seed-1 in every new process), which +// caused intermittent collisions when ctest -j launched the same binary in +// parallel. The PID + a process-local counter + a nanosecond timestamp slice +// is more than sufficient for gtest's handful of fixture paths. std::filesystem::path make_temp_path(const std::string& suffix) { - return std::filesystem::temp_directory_path() / - ("svslam_euroc_test_" + std::to_string(std::rand()) + suffix); + static std::atomic counter{0}; + const auto now_ns = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + const auto id = counter.fetch_add(1); + std::string name = "svslam_euroc_test_" + std::to_string(getpid()) + "_" + + std::to_string(id) + "_" + std::to_string(now_ns) + + suffix; + return std::filesystem::temp_directory_path() / name; } void write_text_file(const std::filesystem::path& path, const std::string& content) { @@ -122,3 +137,139 @@ TEST(EurocDatasetTest, LoadsStereoImagePairs) { std::error_code ec; std::filesystem::remove_all(root, ec); } + +TEST(EurocDatasetTest, LoadsImuDataWhenPresent) { + const auto root = make_temp_path(""); + const auto cam0_data_dir = root / "mav0" / "cam0" / "data"; + const auto imu_dir = root / "mav0" / "imu0"; + std::filesystem::create_directories(cam0_data_dir); + std::filesystem::create_directories(imu_dir); + + write_text_file(root / "mav0" / "cam0" / "sensor.yaml", + "intrinsics: [50.0, 50.0, 4.0, 3.0]\n" + "resolution: [8, 6]\n"); + write_text_file(root / "mav0" / "cam0" / "data.csv", + "#timestamp [ns],filename\n" + "1000000000,1000000000.png\n"); + write_gray_png(cam0_data_dir / "1000000000.png", 8, 6, 17); + + write_text_file(imu_dir / "data.csv", + "#timestamp [ns],w_x,w_y,w_z,a_x,a_y,a_z\n" + "500000000,0.1,0.2,0.3,1.0,2.0,9.81\n" + "1500000000,0.11,0.21,0.31,1.01,2.01,9.82\n" + "2500000000,0.12,0.22,0.32,1.02,2.02,9.83\n"); + + EurocDataset dataset(root.string()); + ASSERT_TRUE(dataset.isValid()) << dataset.error(); + ASSERT_TRUE(dataset.hasImu()); + ASSERT_EQ(dataset.allImu().size(), 3u); + + const auto& first = dataset.allImu().front(); + EXPECT_DOUBLE_EQ(first.timestamp_sec, 0.5); + EXPECT_DOUBLE_EQ(first.gyro.x(), 0.1); + EXPECT_DOUBLE_EQ(first.accel.z(), 9.81); + + const auto between = dataset.getImuBetween(1.0, 2.0); + ASSERT_EQ(between.size(), 1u); + EXPECT_DOUBLE_EQ(between.front().timestamp_sec, 1.5); + + std::error_code ec; + std::filesystem::remove_all(root, ec); +} + +TEST(EurocDatasetTest, ParsesCam0FromImuExtrinsicFromSensorYaml) { + const auto root = make_temp_path(""); + const auto cam0_data_dir = root / "mav0" / "cam0" / "data"; + std::filesystem::create_directories(cam0_data_dir); + + // EuRoC MH_01's actual cam0 T_BS (body=IMU): ~5 cm forward + 6 cm left, + // approximately 90 deg rotation about the IMU's X axis. + write_text_file(root / "mav0" / "cam0" / "sensor.yaml", + "intrinsics: [50.0, 50.0, 4.0, 3.0]\n" + "resolution: [8, 6]\n" + "T_BS:\n" + " cols: 4\n" + " rows: 4\n" + " data: [0.0148655429818, -0.999880929698, 0.00414029679422, " + "-0.0216401454975, 0.999557249008, 0.0149672133247, " + "0.025715529948, -0.064676986768, -0.0257744366974, " + "0.00375618835797, 0.999660727178, 0.00981073058949, " + "0.0, 0.0, 0.0, 1.0]\n"); + write_text_file(root / "mav0" / "cam0" / "data.csv", + "#timestamp [ns],filename\n" + "1000000000,1000000000.png\n"); + write_gray_png(cam0_data_dir / "1000000000.png", 8, 6, 17); + + EurocDataset dataset(root.string()); + ASSERT_TRUE(dataset.isValid()) << dataset.error(); + ASSERT_TRUE(dataset.hasCam0FromImuExtrinsic()); + + const SE3 T_cam_imu = dataset.cam0FromImuExtrinsic(); + const Vec3 translation = T_cam_imu.translation(); + EXPECT_NEAR(translation.x(), -0.0216401454975, 1e-9); + EXPECT_NEAR(translation.y(), -0.064676986768, 1e-9); + EXPECT_NEAR(translation.z(), 0.00981073058949, 1e-9); + + // Rotation is re-orthonormalized via SVD; check it is still a proper SO(3). + const Mat33 R = T_cam_imu.rotationMatrix(); + EXPECT_NEAR((R * R.transpose() - Mat33::Identity()).norm(), 0.0, 1e-9); + EXPECT_NEAR(R.determinant(), 1.0, 1e-9); + + std::error_code ec; + std::filesystem::remove_all(root, ec); +} + +TEST(EurocDatasetTest, ParsesMultiLineTBsArray) { + // Real EuRoC sensor.yaml wraps the 16 T_BS values across four lines. + const auto root = make_temp_path(""); + const auto cam0_data_dir = root / "mav0" / "cam0" / "data"; + std::filesystem::create_directories(cam0_data_dir); + + write_text_file(root / "mav0" / "cam0" / "sensor.yaml", + "intrinsics: [50.0, 50.0, 4.0, 3.0]\n" + "resolution: [8, 6]\n" + "T_BS:\n" + " cols: 4\n" + " rows: 4\n" + " data: [0.01486, -0.99988, 0.00414, -0.02164,\n" + " 0.99956, 0.01497, 0.02572, -0.06468,\n" + " -0.02577, 0.00376, 0.99966, 0.00981,\n" + " 0.0, 0.0, 0.0, 1.0]\n"); + write_text_file(root / "mav0" / "cam0" / "data.csv", + "#timestamp [ns],filename\n" + "1000000000,1000000000.png\n"); + write_gray_png(cam0_data_dir / "1000000000.png", 8, 6, 17); + + EurocDataset dataset(root.string()); + ASSERT_TRUE(dataset.isValid()) << dataset.error(); + ASSERT_TRUE(dataset.hasCam0FromImuExtrinsic()); + const Vec3 t = dataset.cam0FromImuExtrinsic().translation(); + EXPECT_NEAR(t.x(), -0.02164, 1e-5); + EXPECT_NEAR(t.y(), -0.06468, 1e-5); + EXPECT_NEAR(t.z(), 0.00981, 1e-5); + + std::error_code ec; + std::filesystem::remove_all(root, ec); +} + +TEST(EurocDatasetTest, SilentlySkipsMissingImu) { + const auto root = make_temp_path(""); + const auto cam0_data_dir = root / "mav0" / "cam0" / "data"; + std::filesystem::create_directories(cam0_data_dir); + + write_text_file(root / "mav0" / "cam0" / "sensor.yaml", + "intrinsics: [50.0, 50.0, 4.0, 3.0]\n" + "resolution: [8, 6]\n"); + write_text_file(root / "mav0" / "cam0" / "data.csv", + "#timestamp [ns],filename\n" + "1000000000,1000000000.png\n"); + write_gray_png(cam0_data_dir / "1000000000.png", 8, 6, 17); + + EurocDataset dataset(root.string()); + ASSERT_TRUE(dataset.isValid()) << dataset.error(); + EXPECT_FALSE(dataset.hasImu()); + EXPECT_TRUE(dataset.allImu().empty()); + + std::error_code ec; + std::filesystem::remove_all(root, ec); +} diff --git a/tests/test_imu_preintegrator.cc b/tests/test_imu_preintegrator.cc new file mode 100644 index 0000000..c5b11c2 --- /dev/null +++ b/tests/test_imu_preintegrator.cc @@ -0,0 +1,91 @@ +#include + +#include "sensors/imu_preintegrator.h" + +using namespace svslam; + +namespace { + +constexpr double kTolerance = 1e-9; + +} + +TEST(ImuPreintegratorTest, ZeroMotionGivesIdentity) { + ImuPreintegrator p; + // A still IMU on Earth reports +g in its z axis. With zero bias these + // samples integrate into a pure gravity-cancelled delta (accel feeds + // into delta_v/delta_p but rotation stays identity). We supply raw + // samples equal to the bias (zero here) so unbiased accel is zero. + for (int i = 0; i < 100; ++i) { + p.integrate(Vec3::Zero(), Vec3::Zero(), 0.01); + } + EXPECT_NEAR(p.deltaT(), 1.0, 1e-12); + EXPECT_LT((p.deltaR().matrix() - Mat33::Identity()).norm(), kTolerance); + EXPECT_LT(p.deltaV().norm(), kTolerance); + EXPECT_LT(p.deltaP().norm(), kTolerance); +} + +TEST(ImuPreintegratorTest, ConstantAccelProducesExpectedDeltaVAndDeltaP) { + ImuPreintegrator p; + const Vec3 a_body(2.0, 0.0, 0.0); // 2 m/s^2 along body x + const Vec3 w_body = Vec3::Zero(); + const double dt = 0.01; + for (int i = 0; i < 100; ++i) { + p.integrate(a_body, w_body, dt); + } + EXPECT_NEAR(p.deltaT(), 1.0, 1e-12); + // Euler integrator: delta_v = sum of a * dt = a * T + EXPECT_NEAR(p.deltaV().x(), 2.0, 1e-6); + // delta_p under forward Euler with R=I: p = sum_k (v_{k-1} * dt + 0.5 a dt^2) + // Closed form: 0.5 * a * T^2 only holds for continuous integration; + // forward Euler over 100 steps yields a slightly different number. We + // verify the magnitude is within a small tolerance of the analytic. + EXPECT_NEAR(p.deltaP().x(), 0.5 * 2.0 * 1.0, 2e-2); +} + +TEST(ImuPreintegratorTest, PredictAppliesGravityAndInitialState) { + ImuPreintegrator p; + // No IMU samples => identity preintegration of duration 0.5s. + for (int i = 0; i < 50; ++i) { + p.integrate(Vec3::Zero(), Vec3::Zero(), 0.01); + } + const ImuPreintegrator::SO3 R_i; // identity + const Vec3 v_i(1.0, 0.0, 0.0); + const Vec3 p_i(0.0, 0.0, 0.0); + const Vec3 g(0.0, 0.0, -9.81); + + ImuPreintegrator::SO3 R_j; + Vec3 v_j, p_j; + p.predict(R_i, v_i, p_i, g, R_j, v_j, p_j); + + EXPECT_NEAR(p.deltaT(), 0.5, 1e-12); + // R_j == R_i (no rotation) + EXPECT_LT((R_j.matrix() - Mat33::Identity()).norm(), kTolerance); + // v_j = v_i + g * dt + EXPECT_NEAR(v_j.x(), 1.0, 1e-9); + EXPECT_NEAR(v_j.z(), -9.81 * 0.5, 1e-9); + // p_j = p_i + v_i * dt + 0.5 * g * dt^2 + EXPECT_NEAR(p_j.x(), 1.0 * 0.5, 1e-9); + EXPECT_NEAR(p_j.z(), 0.5 * -9.81 * 0.5 * 0.5, 1e-9); +} + +TEST(ImuPreintegratorTest, ResetClearsAccumulatorAndSetsBiases) { + ImuPreintegrator p; + p.integrate(Vec3(1, 2, 3), Vec3(0.1, 0.2, 0.3), 0.01); + p.reset(Vec3(0.5, 0.5, 0.5), Vec3(0.01, 0.01, 0.01)); + EXPECT_DOUBLE_EQ(p.deltaT(), 0.0); + EXPECT_LT(p.deltaV().norm(), kTolerance); + EXPECT_LT(p.deltaP().norm(), kTolerance); + EXPECT_DOUBLE_EQ(p.accelBias().x(), 0.5); + EXPECT_DOUBLE_EQ(p.gyroBias().z(), 0.01); +} + +TEST(ImuPreintegratorTest, BiasSubtractsFromRawMeasurement) { + ImuPreintegrator p(Vec3(1.0, 0.0, 0.0), Vec3::Zero()); + // Raw accel equal to bias => unbiased accel is zero, no motion. + for (int i = 0; i < 50; ++i) { + p.integrate(Vec3(1.0, 0.0, 0.0), Vec3::Zero(), 0.01); + } + EXPECT_LT(p.deltaV().norm(), kTolerance); + EXPECT_LT(p.deltaP().norm(), kTolerance); +} diff --git a/tests/test_optimizer.cc b/tests/test_optimizer.cc index dba3ab9..4cdb7f8 100644 --- a/tests/test_optimizer.cc +++ b/tests/test_optimizer.cc @@ -129,6 +129,201 @@ TEST(OptimizerTest, DepthPriorCostMatchesObservedDepth) { EXPECT_NEAR(residuals[0], 10.0, 1e-9); } +TEST(OptimizerTest, VelocityPreintegrationResidualZeroWhenPredictionMatches) { + // Zero-gravity setup: KF_i at world origin with v=0; KF_j at world (1,0,0) + // with v=(2,0,0). Preintegration says delta_p=(1,0,0), delta_v=(2,0,0) + // over dt=1 s. With accel bias matching the reference, the residual + // must vanish because the prediction is exact. + const double pose_i[7] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + const double pose_j[7] = {-1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + const double vel_i[3] = {0.0, 0.0, 0.0}; + const double vel_j[3] = {2.0, 0.0, 0.0}; + const double ba_i[3] = {0.0, 0.0, 0.0}; // equal to ba_ref → no correction + double residuals[9] = {0.0}; + + VelocityPreintegrationError error( + Vec3(1.0, 0.0, 0.0), Vec3(2.0, 0.0, 0.0), + Eigen::Quaterniond::Identity(), + Vec3::Zero(), 1.0, Vec3::Zero(), + Eigen::Quaterniond::Identity(), Vec3::Zero(), + /*pos_weight=*/10.0, /*vel_weight=*/5.0, /*rot_weight=*/4.0); + ASSERT_TRUE(error(pose_i, pose_j, vel_i, vel_j, ba_i, residuals)); + for (int k = 0; k < 9; ++k) { + EXPECT_NEAR(residuals[k], 0.0, 1e-9) << "residual[" << k << "]"; + } +} + +TEST(OptimizerTest, VelocityPreintegrationResidualAccountsForGravity) { + // Free-fall: v=0 for both KFs, zero body-frame preintegration deltas, + // KF_j still at origin after 1 s → the residual should highlight the + // missing gravity drop. + const double pose_i[7] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + const double pose_j[7] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + const double vel_i[3] = {0.0, 0.0, 0.0}; + const double vel_j[3] = {0.0, 0.0, 0.0}; + const double ba_i[3] = {0.0, 0.0, 0.0}; + double residuals[9] = {0.0}; + + VelocityPreintegrationError error( + Vec3::Zero(), Vec3::Zero(), + Eigen::Quaterniond::Identity(), + Vec3::Zero(), 1.0, + Vec3(0.0, 0.0, -9.81), Eigen::Quaterniond::Identity(), Vec3::Zero(), + /*pos_weight=*/1.0, /*vel_weight=*/1.0, /*rot_weight=*/1.0); + ASSERT_TRUE(error(pose_i, pose_j, vel_i, vel_j, ba_i, residuals)); + EXPECT_NEAR(residuals[2], 4.905, 1e-6); + EXPECT_NEAR(residuals[5], 9.81, 1e-6); + // No rotation motion, identity delta_R → rotation residuals are zero. + EXPECT_NEAR(residuals[6], 0.0, 1e-9); + EXPECT_NEAR(residuals[7], 0.0, 1e-9); + EXPECT_NEAR(residuals[8], 0.0, 1e-9); +} + +TEST(OptimizerTest, VelocityPreintegrationBiasAppliesFirstOrderCorrection) { + // Same stationary-poses setup as the gravity test but now the accel bias + // at KF_i differs from the reference bias used during preintegration. The + // first-order correction should show up as -(bias-ref)*dt in the velocity + // residual and -0.5*(bias-ref)*dt^2 in the position residual. + const double pose_i[7] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + const double pose_j[7] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + const double vel_i[3] = {0.0, 0.0, 0.0}; + const double vel_j[3] = {0.0, 0.0, 0.0}; + const double ba_i[3] = {0.1, 0.0, 0.0}; + double residuals[9] = {0.0}; + + // Pre-integration stored deltas assuming bias = 0; no gravity. + VelocityPreintegrationError error( + Vec3::Zero(), Vec3::Zero(), + Eigen::Quaterniond::Identity(), + Vec3::Zero(), /*dt=*/2.0, + Vec3::Zero(), Eigen::Quaterniond::Identity(), Vec3::Zero(), + /*pos_weight=*/1.0, /*vel_weight=*/1.0, /*rot_weight=*/1.0); + ASSERT_TRUE(error(pose_i, pose_j, vel_i, vel_j, ba_i, residuals)); + // With positions fixed at origin the residual is -(−R*dp_corr) = R*dp_corr. + // R=I here, dp_corr = dp - 0.5*dba*dt^2 = -0.5 * 0.1 * 4 = -0.2 on x. + // residual_pos_x = (p_j-p_i) - 0 - 0 - R*dp_corr = 0 - (-0.2) = 0.2. + EXPECT_NEAR(residuals[0], 0.2, 1e-9); + // residual_vel_x = (v_j-v_i) - 0 - R*dv_corr. dv_corr = 0 - 0.1*2 = -0.2. + // residual_vel_x = 0 - (-0.2) = 0.2. + EXPECT_NEAR(residuals[3], 0.2, 1e-9); +} + +TEST(OptimizerTest, VelocityPreintegrationResidualZeroWhenRotationMatches) { + // Both KFs at the world origin. KF_j is rotated 30° about Z vs KF_i. The + // preintegrated delta_R encodes the same 30° rotation, so the rotation + // residual should be zero. Position + velocity residuals are set up to + // match as well (no motion, no gravity, zero bias). + const double pose_i[7] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + const Eigen::Quaterniond q_wb_j( + Eigen::AngleAxisd(30.0 * M_PI / 180.0, Eigen::Vector3d::UnitZ())); + // pose_j stores T_cw_j, and we want R_wb_j = q_wb_j with T_cam_imu = I + // (so q_wb == q_wc). Thus q_cw_j = q_wc_j^T = q_wb_j^T. + const Eigen::Quaterniond q_cw_j = q_wb_j.conjugate(); + const double pose_j[7] = {0.0, 0.0, 0.0, + q_cw_j.w(), q_cw_j.x(), q_cw_j.y(), q_cw_j.z()}; + const double vel_i[3] = {0.0, 0.0, 0.0}; + const double vel_j[3] = {0.0, 0.0, 0.0}; + const double ba_i[3] = {0.0, 0.0, 0.0}; + double residuals[9] = {0.0}; + + VelocityPreintegrationError error( + Vec3::Zero(), Vec3::Zero(), + q_wb_j, // delta_R matches the rotation from i to j + Vec3::Zero(), /*dt=*/1.0, Vec3::Zero(), + Eigen::Quaterniond::Identity(), Vec3::Zero(), + /*pos_weight=*/1.0, /*vel_weight=*/1.0, /*rot_weight=*/10.0); + ASSERT_TRUE(error(pose_i, pose_j, vel_i, vel_j, ba_i, residuals)); + for (int k = 0; k < 9; ++k) { + EXPECT_NEAR(residuals[k], 0.0, 1e-9) << "residual[" << k << "]"; + } +} + +TEST(OptimizerTest, VelocityPreintegrationResidualPenalizesRotationMismatch) { + // Predicted 30° rotation but actual is 40° — residual should fire with + // magnitude ≈ weight * 10° in radians = weight * 0.1745. + const double pose_i[7] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + const Eigen::Quaterniond q_actual( + Eigen::AngleAxisd(40.0 * M_PI / 180.0, Eigen::Vector3d::UnitZ())); + const Eigen::Quaterniond q_predicted( + Eigen::AngleAxisd(30.0 * M_PI / 180.0, Eigen::Vector3d::UnitZ())); + const Eigen::Quaterniond q_cw_j = q_actual.conjugate(); + const double pose_j[7] = {0.0, 0.0, 0.0, + q_cw_j.w(), q_cw_j.x(), q_cw_j.y(), q_cw_j.z()}; + const double vel_i[3] = {0.0, 0.0, 0.0}; + const double vel_j[3] = {0.0, 0.0, 0.0}; + const double ba_i[3] = {0.0, 0.0, 0.0}; + double residuals[9] = {0.0}; + + const double rot_weight = 5.0; + VelocityPreintegrationError error( + Vec3::Zero(), Vec3::Zero(), + q_predicted, + Vec3::Zero(), /*dt=*/1.0, Vec3::Zero(), + Eigen::Quaterniond::Identity(), Vec3::Zero(), + /*pos_weight=*/1.0, /*vel_weight=*/1.0, rot_weight); + ASSERT_TRUE(error(pose_i, pose_j, vel_i, vel_j, ba_i, residuals)); + // pos and vel components are unaffected by the rotation mismatch here. + for (int k = 0; k < 6; ++k) { + EXPECT_NEAR(residuals[k], 0.0, 1e-6); + } + // The error rotation is 10° about Z; its log (half-angle sin) gives + // 2 * sin(5°) ≈ 0.1745 on the z residual (x, y stay 0). + EXPECT_NEAR(residuals[6], 0.0, 1e-6); + EXPECT_NEAR(residuals[7], 0.0, 1e-6); + EXPECT_NEAR(residuals[8], + rot_weight * 2.0 * std::sin(5.0 * M_PI / 180.0), 1e-6); +} + +TEST(OptimizerTest, BiasAnchorErrorPenalizesNonZeroBias) { + const double bias[3] = {0.1, -0.2, 0.3}; + double residuals[3] = {0.0, 0.0, 0.0}; + + BiasAnchorError error(/*weight=*/2.0); + ASSERT_TRUE(error(bias, residuals)); + EXPECT_NEAR(residuals[0], 0.2, 1e-12); + EXPECT_NEAR(residuals[1], -0.4, 1e-12); + EXPECT_NEAR(residuals[2], 0.6, 1e-12); +} + +TEST(OptimizerTest, BiasRandomWalkErrorMeasuresDifference) { + const double bias_i[3] = {0.1, 0.0, 0.0}; + const double bias_j[3] = {0.15, 0.0, 0.0}; + double residuals[3] = {0.0, 0.0, 0.0}; + + BiasRandomWalkError error(/*weight=*/10.0); + ASSERT_TRUE(error(bias_i, bias_j, residuals)); + EXPECT_NEAR(residuals[0], 0.5, 1e-9); + EXPECT_NEAR(residuals[1], 0.0, 1e-12); + EXPECT_NEAR(residuals[2], 0.0, 1e-12); +} + +TEST(OptimizerTest, VelocityDeltaPriorZeroWhenPositionsMatchVelocityIntegral) { + // KF_i at world origin, KF_j at world (1, 0, 0), velocity 0.5 m/s over 2 s. + const double pose_i[7] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + const double pose_j[7] = {-1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + double residuals[3] = {0.0, 0.0, 0.0}; + + VelocityDeltaPriorError error(Vec3(0.5, 0.0, 0.0), 2.0, /*weight=*/10.0); + ASSERT_TRUE(error(pose_i, pose_j, residuals)); + EXPECT_NEAR(residuals[0], 0.0, 1e-9); + EXPECT_NEAR(residuals[1], 0.0, 1e-9); + EXPECT_NEAR(residuals[2], 0.0, 1e-9); +} + +TEST(OptimizerTest, VelocityDeltaPriorPenalizesJumpBeyondExpectedDelta) { + // Same pair but velocity says we should have moved 2.0 m while we only + // moved 1.0 m — expect a 1.0 m residual on x scaled by weight. + const double pose_i[7] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + const double pose_j[7] = {-1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; + double residuals[3] = {0.0, 0.0, 0.0}; + + VelocityDeltaPriorError error(Vec3(1.0, 0.0, 0.0), 2.0, /*weight=*/5.0); + ASSERT_TRUE(error(pose_i, pose_j, residuals)); + EXPECT_NEAR(residuals[0], 5.0 * (1.0 - 2.0), 1e-9); + EXPECT_NEAR(residuals[1], 0.0, 1e-9); + EXPECT_NEAR(residuals[2], 0.0, 1e-9); +} + TEST(OptimizerTest, GravityPriorCostPenalizesTiltError) { const double identity_pose[7] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}; double residuals[3] = {0.0, 0.0, 0.0}; diff --git a/tests/test_visual_inertial_initializer.cc b/tests/test_visual_inertial_initializer.cc new file mode 100644 index 0000000..51387f0 --- /dev/null +++ b/tests/test_visual_inertial_initializer.cc @@ -0,0 +1,252 @@ +#include + +#include +#include + +#include "core/frame.h" +#include "core/keyframe.h" +#include "core/camera.h" +#include "sensors/imu_preintegration_span.h" +#include "tracking/visual_inertial_initializer.h" + +using namespace svslam; + +namespace { + +// Build a keyframe with a given world pose, timestamp, and IMU span from +// its predecessor (if any). The span is populated with the body-frame +// deltas that make the VI init equations exactly satisfied at the given +// scale / gravity / velocity profile — we invert the Forster equations: +// +// R_wb_i * delta_p = s*(p_wb_j - p_wb_i) - v_i*dt - 0.5*g*dt^2 +// R_wb_i * delta_v = (v_j - v_i) - g*dt +// delta_R = R_wb_i^T * R_wb_j +// +// which gives a noise-free bootstrap scene that the initializer should +// recover to within numerical precision. +struct KfScene { + std::vector kfs; + std::vector velocities_world; + Vec3 gravity_world; + double scale = 1.0; + Vec3 gyro_bias = Vec3::Zero(); +}; + +Keyframe::Ptr makeKeyframeForScene(unsigned long id, double timestamp, + const Camera::Ptr& cam, + const SE3& T_cw) { + // A bare Frame with a few keypoints; the initializer only reads pose + // and span, so the imagery + landmarks don't matter here. + auto frame = std::make_shared(id, timestamp, cam, + cv::Mat::zeros(480, 640, CV_8UC1)); + frame->keypoints_.resize(4); + frame->landmarks_.resize(4); + frame->T_cw_ = T_cw; + auto kf = std::make_shared(frame); + kf->timestamp_ = timestamp; + return kf; +} + +KfScene buildNoiselessEurocLikeScene(std::size_t num_kfs, + double scale_true, + const Vec3& gravity_world, + const SE3& T_cam_imu) { + KfScene scene; + scene.scale = scale_true; + scene.gravity_world = gravity_world; + + auto cam = std::make_shared(458.65, 457.30, 367.22, 248.38); + + const double dt = 0.1; + Vec3 v_world(0.5, 0.2, 0.0); // nonzero so the system is observable + const Vec3 accel_world(0.3, -0.1, 0.0); // constant accel over window + + // Generate body poses R_wb, p_wb in the "true" metric world frame, + // then build the visual map at 1/scale units (the mono median-depth + // rescaling multiplies positions by 1/scale_true by construction). + const Sophus::SO3d R_cb = T_cam_imu.so3(); + const Vec3 t_cb = T_cam_imu.translation(); + const Sophus::SO3d R_bc = R_cb.inverse(); + const Vec3 t_bc = -(R_bc * t_cb); + + std::vector R_wb_list; + std::vector p_wb_list; + std::vector v_list; + + Sophus::SO3d R_wb = Sophus::SO3d(); // identity + Vec3 p_wb(0.0, 0.0, 2.0); // some initial position in metric world + R_wb_list.push_back(R_wb); + p_wb_list.push_back(p_wb); + v_list.push_back(v_world); + + // Small constant angular velocity to exercise the gyro path. + const Vec3 omega_world(0.0, 0.0, 0.05); + + for (std::size_t i = 1; i < num_kfs; ++i) { + const Vec3 v_next = v_world + gravity_world * dt + accel_world * dt; + const Vec3 p_next = p_wb + v_world * dt + 0.5 * (gravity_world + accel_world) * dt * dt; + const Sophus::SO3d R_next = R_wb * Sophus::SO3d::exp(omega_world * dt); + R_wb_list.push_back(R_next); + p_wb_list.push_back(p_next); + v_list.push_back(v_next); + R_wb = R_next; + p_wb = p_next; + v_world = v_next; + } + + // The visual map scale is 1/scale_true (mono median-depth rescale). + for (std::size_t i = 0; i < num_kfs; ++i) { + const Sophus::SO3d R_wc = R_wb_list[i] * R_cb.inverse(); + const Vec3 p_wc_metric = p_wb_list[i] + R_wb_list[i] * t_bc; + const Vec3 p_wc_visual = p_wc_metric / scale_true; + const SE3 T_wc(R_wc, p_wc_visual); + auto kf = makeKeyframeForScene(static_cast(i), + static_cast(i) * dt, cam, + T_wc.inverse()); + scene.kfs.push_back(kf); + scene.velocities_world.push_back(v_list[i]); + } + + // Build spans from metric deltas (preintegration is invariant under + // the visual rescaling — it lives in the body frame). + for (std::size_t i = 1; i < num_kfs; ++i) { + const Vec3 dp = R_wb_list[i - 1].inverse() * + (p_wb_list[i] - p_wb_list[i - 1] - + v_list[i - 1] * dt - + 0.5 * gravity_world * dt * dt); + const Vec3 dv = R_wb_list[i - 1].inverse() * + (v_list[i] - v_list[i - 1] - gravity_world * dt); + const Sophus::SO3d dR = R_wb_list[i - 1].inverse() * R_wb_list[i]; + + auto span = std::make_unique(); + span->delta_R = dR; + span->delta_v = dv; + span->delta_p = dp; + span->dt = dt; + span->bias_accel = Vec3::Zero(); + span->bias_gyro = Vec3::Zero(); + span->from_kf_id = scene.kfs[i - 1]->id_; + span->T_cam_imu = T_cam_imu; + span->valid = true; + scene.kfs[i]->prev_imu_span_ = std::move(span); + } + + return scene; +} + +} // namespace + +TEST(VisualInertialInitializerTest, RecoversScaleAndGravityOnSyntheticScene) { + const double scale_true = 0.25; // visual map 4× the metric magnitude + const Vec3 gravity_world(0.0, 0.0, -9.81); + // Non-identity T_cam_imu so the body-vs-camera math is exercised. + const Sophus::SO3d R_cb(Eigen::Quaterniond( + Eigen::AngleAxisd(0.1, Eigen::Vector3d(0.0, 1.0, 0.0)) * + Eigen::AngleAxisd(-0.2, Eigen::Vector3d(1.0, 0.0, 0.0)))); + const Vec3 t_cb(0.05, -0.02, 0.01); + const SE3 T_cam_imu(R_cb, t_cb); + + auto scene = buildNoiselessEurocLikeScene(20, scale_true, gravity_world, T_cam_imu); + + VisualInertialInitializer::Options opts; + opts.metric_scale = false; + opts.scale_prior_weight = 0.0; // pure LSQ recovery + VisualInertialInitializer vi(opts); + + const auto result = vi.initialize(scene.kfs); + ASSERT_TRUE(result.converged) << "message: " << result.message + << " scale=" << result.scale + << " rot_rms=" << result.rotation_residual_rms + << " lin_rms=" << result.linear_residual_rms; + + // Scale should recover to ≈ scale_true within LSQ tolerance. + EXPECT_NEAR(result.scale, scale_true, 1e-3); + + // Gravity should line up with (0,0,-9.81). Note: the initializer + // always normalises |g| to the expected magnitude, so we check the + // direction. + EXPECT_NEAR(result.gravity_w.norm(), 9.81, 1e-3); + EXPECT_GT(result.gravity_w.normalized().dot(gravity_world.normalized()), + 1.0 - 1e-3); + + // Velocities should closely match the ground truth profile. + ASSERT_EQ(result.velocities.size(), scene.velocities_world.size()); + for (std::size_t i = 0; i < result.velocities.size(); ++i) { + EXPECT_LT((result.velocities[i] - scene.velocities_world[i]).norm(), 1e-3) + << "velocity mismatch at KF " << i; + } +} + +TEST(VisualInertialInitializerTest, RecoversGyroBiasClosedForm) { + const double scale_true = 1.0; // irrelevant here; we just need poses + const Vec3 gravity_world(0.0, 0.0, -9.81); + const Sophus::SO3d R_cb; // identity + const Vec3 t_cb = Vec3::Zero(); + const SE3 T_cam_imu(R_cb, t_cb); + + // Build a baseline scene, then perturb every span's delta_R to + // simulate a nonzero gyro bias recorded during preintegration. The + // closed-form stage-1 solve should back out the bias that explains + // the rotation residual. + auto scene = buildNoiselessEurocLikeScene(15, scale_true, gravity_world, T_cam_imu); + + const Vec3 true_bg(0.01, -0.02, 0.005); // rad/s (EuRoC-scale) + for (std::size_t i = 1; i < scene.kfs.size(); ++i) { + auto& span = *scene.kfs[i]->prev_imu_span_; + // Forster linearization: delta_R(b_ref + db) ≈ delta_R(b_ref) * Exp(-dt * db). + // If we recorded a span at b_ref = 0 but the true gyro bias is + // true_bg, then the clean delta_R_true satisfies + // delta_R_true ≈ delta_R_recorded * Exp(-dt * true_bg), + // so working backwards delta_R_recorded ≈ delta_R_true * Exp(+dt * true_bg). + const Sophus::SO3d perturb = Sophus::SO3d::exp(true_bg * span.dt); + span.delta_R = span.delta_R * perturb; + span.bias_gyro = Vec3::Zero(); // recorded during preint at zero bias + } + + VisualInertialInitializer::Options opts; + // Stage 1 (gyro bias) is independent of the linear scale solve, so we + // let the test pin scale=1 to keep the test focused on the rotation + // side; scale recovery is covered by the dedicated scene test. + opts.metric_scale = true; + opts.rotation_residual_rms_max = 0.5; // accept larger residual here + VisualInertialInitializer vi(opts); + + const auto result = vi.initialize(scene.kfs); + ASSERT_TRUE(result.converged) << "message: " << result.message; + + // The estimated gyro bias should be close to the perturbation bias. + EXPECT_LT((result.gyro_bias - true_bg).norm(), 5e-3) + << "estimated gyro bias = " << result.gyro_bias.transpose() + << ", expected ≈ " << true_bg.transpose(); +} + +TEST(VisualInertialInitializerTest, RejectsWindowWithoutSpans) { + auto cam = std::make_shared(458.65, 457.30, 367.22, 248.38); + std::vector kfs; + for (unsigned long i = 0; i < 5; ++i) { + kfs.push_back(makeKeyframeForScene(i, static_cast(i) * 0.1, cam, + SE3())); + } + + VisualInertialInitializer vi; + const auto result = vi.initialize(kfs); + EXPECT_FALSE(result.converged); + EXPECT_FALSE(result.message.empty()); +} + +TEST(VisualInertialInitializerTest, MetricScaleModePinsScaleToOne) { + const double scale_visual = 1.0; // already metric + const Vec3 gravity_world(0.0, 0.0, -9.81); + const SE3 T_cam_imu; // identity + + auto scene = buildNoiselessEurocLikeScene(15, scale_visual, gravity_world, T_cam_imu); + + VisualInertialInitializer::Options opts; + opts.metric_scale = true; + VisualInertialInitializer vi(opts); + + const auto result = vi.initialize(scene.kfs); + ASSERT_TRUE(result.converged) << "message: " << result.message; + EXPECT_DOUBLE_EQ(result.scale, 1.0); + EXPECT_NEAR(result.gravity_w.norm(), 9.81, 1e-3); +}