gps-denied-onboard/_docs/02_document/components/01_c1_vio/description.md

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 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.

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.12.0 (CVE-2025-53644 mitigation)	KLT pyramidal optical flow + RANSAC for the simple-baseline strategy
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.

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.