mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 12:31:13 +00:00
Update autodev state, architecture documentation, and glossary terms
Transitioned the autodev state to phase 21, reflecting the completion of Step 5 and the drafting of Step 6 epics. Revised the architecture documentation to clarify the roles of the Tile Manager and its components, ensuring accurate representation of the system's operational flow. Updated glossary entries for Flight State and Operator to incorporate recent changes and enhance clarity on component interactions and responsibilities.
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,136 @@
|
||||
# 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.
|
||||
@@ -0,0 +1,176 @@
|
||||
# Test Specification — C1 Visual / Visual-Inertial Odometry
|
||||
|
||||
This file is component-scoped. AC-level coverage is in the suite-level test docs (`_docs/02_document/tests/*.md`) and the canonical traceability map is `_docs/02_document/tests/traceability-matrix.md`. The tables below cite test IDs from those files; the `Component-Internal Tests` section adds C1-specific unit/contract tests that the suite-level scenarios do not cover.
|
||||
|
||||
## Acceptance Criteria Traceability
|
||||
|
||||
| AC ID | Acceptance Criterion (one-line) | Test IDs (suite-level + this file) | Coverage |
|
||||
|-------|---------------------------------|-----------------------------------|----------|
|
||||
| AC-1.3 | Cumulative drift between satellite-anchored fixes <100 m visual / <50 m IMU-fused | FT-P-02, **C1-IT-01** | Covered |
|
||||
| AC-1.4 | Estimate reports 95% covariance + source label | FT-P-03, **C1-IT-02** | Covered |
|
||||
| AC-2.1a | Frame-to-frame registration ≥95% on normal segments | FT-P-04, **C1-IT-03** | Covered |
|
||||
| AC-2.2 (frame-to-frame portion) | MRE <1 px frame-to-frame | FT-P-05, **C1-IT-04** | Covered |
|
||||
| AC-3.2 | Tolerate sharp turns; recovery via satellite re-loc | FT-P-07, FT-N-02 | Covered (C1 contributes track-loss detection) |
|
||||
| AC-4.1 | E2E latency <400 ms p95 | NFT-PERF-01 (Tier-2), **C1-PT-01** | Covered |
|
||||
| AC-5.1 | Init from FC EKF's last valid GPS + IMU-extrapolated | FT-P-11, **C1-IT-05** | Covered |
|
||||
| AC-5.3 | On reboot, re-init from FC IMU-extrapolated pose | NFT-RES-02, **C1-IT-06** | Covered |
|
||||
|
||||
---
|
||||
|
||||
## Component-Internal Tests
|
||||
|
||||
### C1-IT-01: VioStrategy contract — `process_frame` honest covariance under degradation
|
||||
|
||||
**Summary**: every concrete `VioStrategy` implementation (Okvis2, VinsMono, KltRansac) must produce a 6×6 covariance whose norm grows monotonically when feature tracking degrades.
|
||||
|
||||
**Traces to**: AC-1.3, AC-1.4
|
||||
|
||||
**Description**: feed each strategy a synthetic 60 s nav-camera + IMU sequence with a controlled feature-loss event at t=30 s (50% feature drop). Assert that `pose_covariance_6x6` Frobenius norm before t=30 s is below the steady-state threshold and rises monotonically for ≥3 s after the event. No strategy may emit a tightened covariance during a degradation event (catches honest-covariance-violation regressions).
|
||||
|
||||
**Input data**: `tests/fixtures/synthetic_vio/normal_then_feature_drop_60s/` (nav-cam frames at 3 Hz + IMU at 200 Hz; feature-loss event injected via image masking).
|
||||
|
||||
**Expected result**: `||cov||_F` curve has a rising shoulder ≥1.5× steady-state norm within 3 s of the event for all three strategies; `VioHealth.state` transitions `TRACKING → DEGRADED` within the same window.
|
||||
|
||||
**Max execution time**: 30 s per strategy on Tier-1.
|
||||
**Dependencies**: helper `ImuPreintegrator` (shared with C5).
|
||||
|
||||
---
|
||||
|
||||
### C1-IT-02: `VioOutput` schema invariants
|
||||
|
||||
**Summary**: every `VioOutput` carries a 6×6 SPD covariance and a non-empty `feature_quality`.
|
||||
|
||||
**Traces to**: AC-1.4
|
||||
|
||||
**Description**: drive each strategy through 100 frames of a synthetic loop; for each emitted `VioOutput`, assert (a) `pose_covariance_6x6` is symmetric and positive-definite, (b) `feature_quality.tracked + new ≥ 0`, (c) `frame_id` matches the input `NavCameraFrame.frame_id`. Any single violation fails the test.
|
||||
|
||||
**Input data**: `tests/fixtures/synthetic_vio/loop_100f/`.
|
||||
|
||||
**Expected result**: 100/100 frames pass all invariants per strategy.
|
||||
|
||||
**Max execution time**: 10 s.
|
||||
|
||||
---
|
||||
|
||||
### C1-IT-03: KltRansac mandatory simple-baseline registration ≥95%
|
||||
|
||||
**Summary**: the simple-baseline strategy must hit AC-2.1a's 95% threshold on the Derkachi normal segment so the engine rule's mandatory baseline is met.
|
||||
|
||||
**Traces to**: AC-2.1a (engine rule)
|
||||
|
||||
**Description**: replay the Derkachi normal-segment fixture (the same 60 stills C8 fixture that FT-P-04 uses) through the KltRansac strategy only; count frames with `VioHealth.state == TRACKING` at emission. Pass if ≥95%.
|
||||
|
||||
**Input data**: `tests/fixtures/flight_derkachi/normal_segment_60_stills/`.
|
||||
|
||||
**Expected result**: tracked frame ratio ≥ 0.95.
|
||||
|
||||
**Max execution time**: 60 s.
|
||||
|
||||
---
|
||||
|
||||
### C1-IT-04: Frame-to-frame MRE bound
|
||||
|
||||
**Summary**: each strategy's per-frame mean reprojection error stays under 1 px on normal segments.
|
||||
|
||||
**Traces to**: AC-2.2 (frame-to-frame portion)
|
||||
|
||||
**Description**: same Derkachi normal-segment fixture as C1-IT-03. Compute MRE per frame from the strategy's internal residual; assert MRE p95 < 1 px per AC-2.2.
|
||||
|
||||
**Input data**: as above.
|
||||
|
||||
**Expected result**: MRE p95 < 1 px for Okvis2 + KltRansac (production-default + simple-baseline). VinsMono is research-only and exempt from MRE bound (only IT-12 comparative-study coverage).
|
||||
|
||||
**Max execution time**: 60 s.
|
||||
|
||||
---
|
||||
|
||||
### C1-IT-05: Warm-start from `WarmStartPose` converges within configured budget
|
||||
|
||||
**Summary**: `reset_to_warm_start` followed by 5 frames of input must converge to `state == TRACKING` for every strategy.
|
||||
|
||||
**Traces to**: AC-5.1
|
||||
|
||||
**Description**: prepare a `WarmStartPose` derived from the FC EKF's last-valid-GPS fixture; call `reset_to_warm_start`, then push 5 frames of normal-segment input; assert health transitions `INIT → TRACKING` within 5 frames.
|
||||
|
||||
**Input data**: `tests/fixtures/flight_derkachi/takeoff_warmstart/`.
|
||||
|
||||
**Expected result**: TRACKING by frame 5 for all three strategies.
|
||||
|
||||
**Max execution time**: 5 s.
|
||||
|
||||
---
|
||||
|
||||
### C1-IT-06: F8 reboot recovery via warm-start hint
|
||||
|
||||
**Summary**: simulate a mid-flight reboot — the strategy must re-init from the warm-start hint without crashing or producing covariance < pre-reboot.
|
||||
|
||||
**Traces to**: AC-5.3
|
||||
|
||||
**Description**: run normal-segment input for 30 s; capture last `VioOutput`; reset the strategy with that pose as `WarmStartPose`; resume input; assert next 5 emitted `VioOutput` have `pose_covariance_6x6` Frobenius norm ≥ the pre-reboot value (no fake confidence after reboot).
|
||||
|
||||
**Input data**: as C1-IT-03.
|
||||
|
||||
**Expected result**: pass per assertion above for all three strategies.
|
||||
|
||||
**Max execution time**: 60 s.
|
||||
|
||||
---
|
||||
|
||||
## Performance Tests
|
||||
|
||||
### C1-PT-01: per-frame latency budget on Tier-2
|
||||
|
||||
**Summary**: `process_frame` p95 latency on Jetson under nominal thermal.
|
||||
|
||||
**Traces to**: AC-4.1 (component-level partition; suite-level NFT-PERF-01 owns the e2e budget).
|
||||
|
||||
**Load scenario**:
|
||||
- Single ingest thread, 3 Hz frame rate, 10 min replay of Derkachi normal segment.
|
||||
- Concurrent C2 backbone forward pass running on the same Jetson (realistic load contention).
|
||||
|
||||
**Expected results**:
|
||||
|
||||
| Metric | Target | Failure Threshold |
|
||||
|--------|--------|-------------------|
|
||||
| `process_frame` latency p50 | ≤ 25 ms (Okvis2 production-default) | 60 ms |
|
||||
| `process_frame` latency p95 | ≤ 80 ms | 120 ms |
|
||||
| Throughput | ≥ 3 Hz sustained | < 2.5 Hz |
|
||||
|
||||
**Resource limits**:
|
||||
- CPU: ≤ 30% of one core (Okvis2 is multi-threaded internally; bound at 30% per ADR-002 budget partition).
|
||||
- Memory: ≤ 1.5 GB resident.
|
||||
|
||||
---
|
||||
|
||||
## Security Tests
|
||||
|
||||
C1 has no externally-reachable surface (internal-only component); suite-level NFT-SEC-02 (no in-flight egress) and NFT-SEC-05 (DNS blackholing) cover the airborne process broadly. No C1-specific security tests.
|
||||
|
||||
---
|
||||
|
||||
## Acceptance Tests
|
||||
|
||||
C1 contributes to AC-1.3 / 1.4 / 2.1a / 5.1 / 5.3 via the suite-level FT scenarios cited in the traceability table. No additional C1-only acceptance tests are needed.
|
||||
|
||||
---
|
||||
|
||||
## Test Data Management
|
||||
|
||||
**Required test data**:
|
||||
|
||||
| Data Set | Description | Source | Size |
|
||||
|----------|-------------|--------|------|
|
||||
| `synthetic_vio/normal_then_feature_drop_60s/` | 60 s nav-cam + IMU with controlled feature-loss event at t=30 s | generated by `scripts/gen_synthetic_vio.py` (deterministic seed) | ~50 MB |
|
||||
| `synthetic_vio/loop_100f/` | 100-frame synthetic closed loop for invariant checks | generated, deterministic | ~30 MB |
|
||||
| `flight_derkachi/normal_segment_60_stills/` | the project's canonical normal-segment fixture | curated subset of Derkachi raw drop | ~80 MB |
|
||||
| `flight_derkachi/takeoff_warmstart/` | last-valid-GPS + IMU window from FC EKF for warm-start tests | recorded once, replayed | ~5 MB |
|
||||
|
||||
**Setup procedure**:
|
||||
1. Run `scripts/gen_synthetic_vio.py` once per fixture to populate `tests/fixtures/synthetic_vio/`.
|
||||
2. Mount `tests/fixtures/flight_derkachi/` from the project's data archive (read-only).
|
||||
|
||||
**Teardown procedure**:
|
||||
1. Synthetic fixtures persist between runs (deterministic; no per-run mutation).
|
||||
2. The Derkachi fixture is read-only; nothing to clean up.
|
||||
|
||||
**Data isolation strategy**: every test runs in its own temp directory under `tests/tmp/c1/<test-id>/`; per-strategy state is constructed fresh in each test (no shared state across tests).
|
||||
Reference in New Issue
Block a user