# C1 — Visual / Visual-Inertial Odometry

## 1. High-Level Overview

**Purpose**: produce a per-frame relative pose SE(3) + 6×6 covariance + IMU bias estimate + feature-quality summary from the nav-camera frame and the FC IMU/attitude window, fusing visual and inertial cues without any external (satellite) reference.

**Architectural Pattern**: Strategy — `VioStrategy` interface with three concrete implementations (Okvis2 nominal production-default, VinsMono research-only, KltRansac mandatory simple-baseline), constructor-injected at the composition root (ADR-009), build-time gated by per-implementation CMake `BUILD_*` flags (ADR-002), runtime selection by config at startup (ADR-001), not hot-swappable mid-flight.

**Cycle-1 operational reality**: the airborne binary ships with **`KltRansac` as the production-default selection** while the OKVIS2 + VINS-Mono native wirings are parked as Tier-2 follow-ups (`AZ-592` for AZ-332 OKVIS2; `AZ-593` for AZ-333 VINS-Mono — see `FINAL_report.md` § Cycle 1 Implementation Status). Both higher-fidelity strategies have their Python facade + pybind11 binding skeleton + `_STRATEGY_REGISTRY` registration in place; the first `process_frame` call into the OKVIS2/VINS-Mono native side raises until the upstream wiring (`okvis::ThreadedSlam` / VINS-Mono ROS-strip) lands. ADR-001 / ADR-002 remain correct — the seam exists, the build-flag gating works — only the operational default-selection shifts. Runtime selection of `okvis2` or `vins_mono` via config currently raises `StrategyNotAvailableError` from `runtime_root/vio_factory.py` until their `BUILD_*` flag is ON.

**Upstream dependencies**:
- Camera ingest thread → `NavCameraFrame` (3 Hz nominal, drop-oldest queue).
- C8 FC adapter inbound side → `ImuWindow` (100–200 Hz, time-aligned to frame timestamp).
- Camera calibration artifact (loaded once at startup; passed in via constructor).

**Downstream consumers**:
- C5 StateEstimator (consumes `VioOutput` for the iSAM2 `BetweenFactorPose3` + IMU bias prior).
- F8 Companion-reboot recovery (uses last `VioOutput` as warm-start hint when re-entering the per-frame loop).

## 2. Internal Interfaces

### Interface: `VioStrategy`

| Method | Input | Output | Async | Error Types |
|--------|-------|--------|-------|-------------|
| `process_frame` | `NavCameraFrame, ImuWindow, CameraCalibration` | `VioOutput` | No (called on the camera ingest hot path) | `VioInitializingError`, `VioDegradedError`, `VioFatalError` |
| `reset_to_warm_start` | `WarmStartPose` | `None` | No | `VioFatalError` |
| `health_snapshot` | `()` | `VioHealth` | No | — |

**Input DTOs**:
```
NavCameraFrame:
  frame_id:           uuid (required) — monotonic per-flight
  capture_timestamp:  monotonic_ns (required) — companion clock; FC clock cross-sync via C8
  pixels:             ndarray[H=3648, W=5472, C=3, dtype=uint8] (required) — 3 Hz nominal
  camera_id:          string (required) — matches the loaded calibration artifact

ImuWindow:
  start_t_ns:         monotonic_ns (required)
  end_t_ns:           monotonic_ns (required) — should bracket the frame timestamp
  samples:            list[ImuSample] (required) — accel + gyro at 100–200 Hz

WarmStartPose:
  body_T_world:       SE3 (required) — initial pose hint, e.g. from F2 takeoff load (AC-5.1)
  velocity_b:         Vector3 (required, m/s)
  bias:               ImuBias (required) — accel + gyro bias seed
```

**Output DTOs**:
```
VioOutput:
  frame_id:               uuid — echoes input
  relative_pose_T:        SE3 — body-frame motion since last keyframe
  pose_covariance_6x6:    Matrix6 — honest ESKF or factor-graph covariance per concrete strategy
  imu_bias:               ImuBias — current accel + gyro bias estimate
  feature_quality:        FeatureQuality — tracked/lost feature counts, mean parallax, MRE
  emitted_at:             monotonic_ns

VioHealth:
  state:                 enum {INIT, TRACKING, DEGRADED, LOST}
  consecutive_lost:      int
  bias_norm:             float — used by C5 quality_metadata + AC-NEW-8 spoof gate
```

## 3. External API Specification

Not applicable — internal-only component, no HTTP/gRPC surface.

## 4. Data Access Patterns

Stateless w.r.t. persistent storage. Each strategy holds **in-memory** state only:
- Sliding window of N keyframes (concrete strategy decides N).
- IMU bias and velocity state.
- Feature track buffer.

No database access, no cache layer beyond the in-process keyframe window.

## 5. Implementation Details

**Algorithmic Complexity**: per-frame cost is dominated by feature extraction + matching; `O(F)` in feature count for KltRansac, `O(F·log K)` for Okvis2 sliding-window optimisation across K keyframes (D-C5-3 sets K=10–20).

**State Management**: per-instance in-memory (window of keyframes, IMU bias, velocity). The strategy lives for the duration of a flight; reset on `reset_to_warm_start` for F8 reboot recovery.

**Key Dependencies**:

| Library | Version | Purpose |
|---------|---------|---------|
| OKVIS2 (C++) | upstream HEAD pinned per Plan-phase | Production-default tightly-coupled VIO; BSD-3-Clause |
| VINS-Mono (C++) | upstream HEAD pinned per Plan-phase | Research-only loosely-coupled VIO for IT-12 comparative study; behind `BUILD_VINS_MONO` |
| OpenCV | `>=4.11.0.86,<4.12` (cycle-1 relaxed pin; D-CROSS-CVE-1 deferred — see `_docs/_process_leftovers/2026-05-11_d_cross_cve_1_opencv_pin_deferred.md`) | KLT pyramidal optical flow + RANSAC for the KltRansac strategy (cycle-1 production-default) |
| Eigen | matches OKVIS2 / GTSAM pin | Lie-algebra math for SE(3) + 6×6 covariance |
| pybind11 | matches OKVIS2 / VINS-Mono build | Python bindings for the C++ strategies |

**Error Handling Strategy**:
- `VioInitializingError`: state = INIT, no `VioOutput` emitted, C5 falls back to FC IMU prior — no MAVLink emission.
- `VioDegradedError`: state = DEGRADED, `VioOutput` emitted with inflated covariance, C5 down-weights.
- `VioFatalError`: state = LOST after configurable consecutive frames; AC-5.2 fallback path triggered (FC IMU-only after 3 s).
- No retries inside `process_frame` — the caller is responsible for handling drop-oldest queue semantics on the hot path.

## 6. Extensions and Helpers

| Helper | Purpose | Used By |
|--------|---------|---------|
| `ImuPreintegrator` | shared GTSAM `CombinedImuFactor` preintegration buffer | C1, C5 (both consume the same IMU window) |
| `SE3Utils` | SE(3) ↔ pose-matrix conversion, Lie-algebra exponential/logarithm | C1, C4, C5 |

## 7. Caveats & Edge Cases

**Known limitations**:
- Pure VIO drifts unbounded over time; AC-1.3 cumulative-drift bound (<100 m visual / <50 m IMU-fused between satellite anchors) is met *only* in cooperation with C2/C3/C4 anchors, not by C1 alone.
- Sharp turns with <5% frame overlap (RESTRICT-UAV-3) cause feature-track loss in all three strategies; F6 satellite re-localization is the recovery path.

**Potential race conditions**:
- The camera ingest thread is the sole producer; C5 is the sole consumer. Concurrent calls to `process_frame` on a single strategy instance are forbidden — enforce in the composition root by binding one strategy instance to the camera ingest thread.

**Performance bottlenecks**:
- Okvis2 sliding-window optimisation can spike to 80–120 ms on a thermally-throttled Jetson; D-CROSS-LATENCY-1 hybrid auto-degrades C4 covariance recovery (not C1) to free budget. (Behaviour is documented for when AZ-592 wires the OKVIS2 native side; in cycle-1, KltRansac is the active backend and its per-frame cost is `O(F)` only.)

**Cycle-1 Tier-2 follow-up dependencies**:
- AZ-592 (parked from AZ-332): wires `okvis::ThreadedSlam` into `_native/okvis2_binding.cpp` and lands the OKVIS2 CI matrix (Ceres + vendored submodules) + Tier-2 Jetson validation against Derkachi-class fixtures. Until this lands, requesting `config.vio.strategy="okvis2"` raises `StrategyNotAvailableError` regardless of `BUILD_OKVIS2`.
- AZ-593 (parked from AZ-333): finalises the de-ROSified VINS-Mono upstream pin (HKUST + in-tree ROS-strip vs. community fork) and wires the `vins_estimator::Estimator` into `_native/vins_mono_binding.cpp`. Until this lands, requesting `config.vio.strategy="vins_mono"` raises `StrategyNotAvailableError` regardless of `BUILD_VINS_MONO`.

## 8. Dependency Graph

**Must be implemented after**: C7 (TRT/ONNX runtime is not on C1's path, but `ImuPreintegrator` shares GTSAM with C5 and is built alongside C5), C13 (FDR sink for VioHealth telemetry).

**Can be implemented in parallel with**: C2, C3, C6 — independent code paths.

**Blocks**: C5 (no fusion without `VioOutput`), C4 (no per-frame relative-pose prior), F3 / F5 / F6 (every per-frame flow consumes `VioOutput`).

## 9. Logging Strategy

| Log Level | When | Example |
|-----------|------|---------|
| ERROR | `VioFatalError` raised; AC-5.2 path imminent | `VIO LOST after 9 consecutive frames; strategy=okvis2` |
| WARN | `VioDegradedError`; covariance inflation > 2× steady-state | `VIO degraded: parallax=0.02, mre=4.1px, bias_norm=0.18` |
| INFO | Strategy init complete; warm-start applied; state transitions | `VIO ready: strategy=okvis2, calibration=adti20.unit-7` |
| DEBUG | Per-frame keyframe decision, feature-track count | `VIO frame=12345 tracked=187 new=42 keyframe=true` |

**Log format**: structured JSON via the project's shared logger; no plaintext.

**Log storage**: stdout (Tier-1) / journald (Tier-2 dev) / FDR via C13 (production). Per-frame DEBUG logs are never persisted to FDR — they go to stdout/journald only.