mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 08:51:12 +00:00
[AZ-334] C1 KLT/RANSAC strategy — engine-rule simple-baseline VIO
Implement KltRansacStrategy, the ADR-002 engine-rule mandatory simple-baseline VioStrategy for E-C1. Pure-Python facade over OpenCV's cv2.goodFeaturesToTrack / calcOpticalFlowPyrLK / findEssentialMat / recoverPose pipeline — no C++/pybind11 binding by design so a Tier-0 workstation runs the strategy with `pip install opencv-python` and the BUILD_KLT_RANSAC=ON gate alone. Constructor + state machine + FDR transition spine mirror Okvis2Strategy + VinsMonoStrategy so the AZ-331 factory + IT-12 comparative harness treat all three as drop-in substitutable; the duplication is the consolidation target now formally in scope for the next cumulative review (batches 52-54). AC coverage: AC-1..AC-11 + NFR-perf mapped to passing tests (25 tests, 23 pass + 2 tier-2 skipped on dev/CI runners; all 25 pass under GPS_DENIED_TIER=2). Honest-covariance invariant (AC-9) implemented as residual-scatter / (N_inliers - 5) with an inlier- count penalty — no client-side floor or smoother; cov Frobenius grows monotonically across DEGRADED. Camera-agnostic source (AC-11) enforced by CI-grep gate that excludes docstring text. Test-Run Cadence: focused suite tests/unit/c1_vio/ green (95 passed, 6 skipped); config-loader + compose-root suites green; full-suite gate deferred to Step 16 per implement skill. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,177 @@
|
|||||||
|
# Batch 54 — Cycle 1 Report
|
||||||
|
|
||||||
|
**Date**: 2026-05-14
|
||||||
|
**Tasks**: AZ-334 (C1 KLT/RANSAC Strategy)
|
||||||
|
**Verdict**: COMPLETE — PASS_WITH_WARNINGS
|
||||||
|
|
||||||
|
## Summary
|
||||||
|
|
||||||
|
Implemented `KltRansacStrategy`, the ADR-002 engine-rule mandatory
|
||||||
|
simple-baseline VIO for C1. Pure-Python facade over OpenCV's
|
||||||
|
`cv2.goodFeaturesToTrack` / `cv2.calcOpticalFlowPyrLK` /
|
||||||
|
`cv2.findEssentialMat` / `cv2.recoverPose` pipeline — no C++/pybind11
|
||||||
|
binding by design so a Tier-0 workstation can run the strategy with
|
||||||
|
`pip install opencv-python` and the AZ-331 factory's
|
||||||
|
`BUILD_KLT_RANSAC=ON` gate. Mirrors the AZ-332 OKVIS2 + AZ-333
|
||||||
|
VINS-Mono facade pattern on the orchestration spine so the AZ-331
|
||||||
|
factory + IT-12 comparative harness treat all three strategies as
|
||||||
|
drop-in substitutable.
|
||||||
|
|
||||||
|
## Files added / modified
|
||||||
|
|
||||||
|
### Added (3)
|
||||||
|
|
||||||
|
- `src/gps_denied_onboard/components/c1_vio/klt_ransac.py` —
|
||||||
|
Python facade, ~770 lines.
|
||||||
|
- `tests/unit/c1_vio/test_klt_ransac_strategy.py` — AC-1..AC-11 +
|
||||||
|
NFR-perf + KltRansacConfig validation tests, ~990 lines, 25 tests
|
||||||
|
(23 pass + 2 tier-2 skipped on dev/CI runners).
|
||||||
|
- `_docs/03_implementation/reviews/batch_54_review.md` — code review
|
||||||
|
report.
|
||||||
|
|
||||||
|
### Modified (4)
|
||||||
|
|
||||||
|
- `src/gps_denied_onboard/components/c1_vio/config.py` — add
|
||||||
|
`KltRansacConfig` dataclass + `klt_ransac` field on `C1VioConfig`;
|
||||||
|
`__all__` updated. (Pre-existing `KNOWN_STRATEGIES` already had
|
||||||
|
`klt_ransac` from AZ-331.)
|
||||||
|
- `src/gps_denied_onboard/components/c1_vio/__init__.py` — export
|
||||||
|
`KltRansacConfig`.
|
||||||
|
- `cpp/klt_ransac/CMakeLists.txt` — replace AZ-263 placeholder with
|
||||||
|
a deliberate "pure-Python; no native target" STATUS message so
|
||||||
|
the build graph stays symmetric with `cpp/okvis2/` and
|
||||||
|
`cpp/vins_mono/` while the `BUILD_KLT_RANSAC=ON` flag only gates
|
||||||
|
the Python module import at the AZ-331 composition-root factory.
|
||||||
|
- `tests/unit/c1_vio/test_protocol_conformance.py` — introduce the
|
||||||
|
`_STRATEGIES_WITHOUT_NATIVE_BINDING` category and route
|
||||||
|
`klt_ransac` through it inside
|
||||||
|
`test_ac5_build_vio_strategy_flag_on_but_module_missing` so the
|
||||||
|
parametrised factory test correctly handles the pure-Python
|
||||||
|
shape (no native binding to fail on; the construction should
|
||||||
|
succeed and return a `VioStrategy` instance instead).
|
||||||
|
|
||||||
|
## AC coverage (AC-1..AC-11 + NFR-perf)
|
||||||
|
|
||||||
|
All 11 ACs + NFR-perf mapped to passing tests (see
|
||||||
|
`batch_54_review.md` Phase 2 table). AC-9 + NFR-perf are
|
||||||
|
tier-2-tagged per the task spec; they skip on macOS dev + GitHub
|
||||||
|
Actions Linux runner with `Tier-2-only test; set GPS_DENIED_TIER=2
|
||||||
|
to run`. AC-3 / AC-6 / AC-9 / AC-10 / AC-11 / NFR-perf monkeypatch
|
||||||
|
`cv2.findEssentialMat` + `cv2.recoverPose` +
|
||||||
|
`RansacFilter.filter_correspondences` to deterministic values so
|
||||||
|
the unit suite exercises the FACADE's state machine without
|
||||||
|
depending on real OpenCV geometry on synthetic correspondences;
|
||||||
|
real-geometry validation lives in C1-IT-12 (Jetson Tier-2 fixture).
|
||||||
|
|
||||||
|
## Test results
|
||||||
|
|
||||||
|
### Focused suite — `tests/unit/c1_vio/`
|
||||||
|
|
||||||
|
```
|
||||||
|
95 passed, 6 skipped in 1.67s
|
||||||
|
```
|
||||||
|
|
||||||
|
The 6 skips are the 2 OKVIS2 tier-2 tests + the 2 VINS-Mono tier-2
|
||||||
|
tests + the 2 new KLT/RANSAC tier-2 tests (`test_ac9_*` and
|
||||||
|
`test_nfr_perf_*`).
|
||||||
|
|
||||||
|
### Adjacent regression — config / compose-root
|
||||||
|
|
||||||
|
```
|
||||||
|
17 passed in 1.96s
|
||||||
|
```
|
||||||
|
|
||||||
|
(`test_az269_config_loader.py`, `test_az270_compose_root.py` — all
|
||||||
|
green; the new `KltRansacConfig` registration did not break the
|
||||||
|
schema-loader or compose-root layered-import guards.)
|
||||||
|
|
||||||
|
### Tier-2 verification
|
||||||
|
|
||||||
|
```
|
||||||
|
GPS_DENIED_TIER=2 pytest tests/unit/c1_vio/test_klt_ransac_strategy.py
|
||||||
|
→ 25 passed in 0.43s
|
||||||
|
```
|
||||||
|
|
||||||
|
All 25 tests pass under the tier-2 gate (including AC-9 honest-
|
||||||
|
covariance monotonicity over 48 synthetic frames + NFR-perf p95
|
||||||
|
record).
|
||||||
|
|
||||||
|
### Full suite
|
||||||
|
|
||||||
|
Deferred per the implement skill's Test-Run Cadence — the full
|
||||||
|
unit-suite gate runs exactly once at Step 16 (end of implementation
|
||||||
|
phase), not per-batch. Focused tests + cumulative review (every K
|
||||||
|
batches) catch cross-batch regressions before then.
|
||||||
|
|
||||||
|
## Architectural decisions
|
||||||
|
|
||||||
|
- **No native binding by design**: KLT/RANSAC is pure Python over
|
||||||
|
OpenCV's Python bindings. The `cpp/klt_ransac/CMakeLists.txt`
|
||||||
|
placeholder is preserved (with an explanatory STATUS message) for
|
||||||
|
build-graph symmetry with `cpp/okvis2/` and `cpp/vins_mono/`; the
|
||||||
|
`BUILD_KLT_RANSAC=ON` flag only gates the Python module import at
|
||||||
|
the AZ-331 composition-root factory.
|
||||||
|
- **Constructor shape matches factory**: `KltRansacStrategy(config,
|
||||||
|
*, fdr_client, clock=None)` mirrors `Okvis2Strategy` +
|
||||||
|
`VinsMonoStrategy` so the AZ-331 factory invokes all three via the
|
||||||
|
same call shape. The task spec's illustrative constructor
|
||||||
|
(with explicit injection of `CameraCalibration` / `ImuPreintegrator`
|
||||||
|
/ `RansacFilter` / `Logger`) was deliberately not adopted because
|
||||||
|
it would diverge from the existing factory contract; the same
|
||||||
|
dependencies are resolved internally instead — `RansacFilter` is
|
||||||
|
static (AZ-282 stateless helper) and `ImuPreintegrator` is
|
||||||
|
constructed lazily on the first `process_frame` call (it needs
|
||||||
|
the per-call `CameraCalibration` which is not available at
|
||||||
|
construction time).
|
||||||
|
- **Honest covariance, AC-9 compliant**: per-frame covariance =
|
||||||
|
`np.eye(6) * (sigma_sq + inlier_penalty) / max(inlier_count - 5, 1)`
|
||||||
|
where `sigma_sq = median_residual_px**2` and `inlier_penalty =
|
||||||
|
threshold_px / max(inlier_count, 1)`. No client-side floor or
|
||||||
|
smoother; the formula grows monotonically as inliers drop or
|
||||||
|
residuals scatter. Tier-2 test walks 48 scripted-inlier frames
|
||||||
|
through the DEGRADED window and verifies monotonicity.
|
||||||
|
- **Camera agnostic, AC-11 enforced**: no `adti20` / `adti26`
|
||||||
|
literals in executable source (docstring mentions are excluded
|
||||||
|
from the CI grep via AST-aware stripping in the test). The
|
||||||
|
per-call `CameraCalibration` argument carries intrinsics; the
|
||||||
|
same code path produces sensible `VioOutput` for two distinct
|
||||||
|
calibrations (different f, cx, cy).
|
||||||
|
- **State machine mirrors OKVIS2 / VINS-Mono**: `INIT` until
|
||||||
|
`warm_start_max_frames` is exhausted, then `TRACKING` if inlier
|
||||||
|
count ≥ `min_features_for_pose`, else `DEGRADED`; sustained
|
||||||
|
pose-recovery failure for `lost_frame_threshold` consecutive
|
||||||
|
frames raises `VioFatalError` with state == `LOST`. Exactly one
|
||||||
|
`vio.health` FDR record per transition (AC-10).
|
||||||
|
- **Error envelope closed**: every OpenCV `cv2.error` is caught at
|
||||||
|
each call site and rewrapped into `VioFatalError` with
|
||||||
|
`__cause__` chaining (AC-4). `RansacFilterError` +
|
||||||
|
`ImuPreintegrationError` are also caught and rewrapped.
|
||||||
|
|
||||||
|
## Out of scope / deferred
|
||||||
|
|
||||||
|
- Real geometry validation against Derkachi fixtures — Tier-2
|
||||||
|
follow-up; C1-IT-12 binds KLT/RANSAC alongside OKVIS2.
|
||||||
|
- Warm-start hint persistence (AZ-335) — separate batch.
|
||||||
|
- Strategy-facade consolidation hygiene PBI — now formally in scope
|
||||||
|
for the cumulative review covering batches 52-54 (next trigger).
|
||||||
|
All three strategy shapes (OKVIS2, VINS-Mono, KLT/RANSAC) are
|
||||||
|
now visible so the right factoring (template-method base class
|
||||||
|
for the orchestration spine + per-strategy geometry hook) can be
|
||||||
|
scoped without over-fitting.
|
||||||
|
|
||||||
|
## Honest-covariance numeric envelope (AC-9 evidence)
|
||||||
|
|
||||||
|
Tier-2 test walks the following inlier-count sequence through the
|
||||||
|
DEGRADED gate (`min_features_for_pose=50`):
|
||||||
|
|
||||||
|
| Frame range | Inlier count | State | Cov Frobenius (approx) |
|
||||||
|
|-------------|--------------|----------|------------------------|
|
||||||
|
| 1 | first-frame | INIT | 24.49 (10·√6) |
|
||||||
|
| 2..29 | 80 | TRACKING | ≈ 0.0056 |
|
||||||
|
| 30 | 40 | DEGRADED | ≈ 0.013 |
|
||||||
|
| 31..43 | 35..12 | DEGRADED | strictly increasing |
|
||||||
|
| 44..49 | 10 | DEGRADED | ≈ 0.127 |
|
||||||
|
|
||||||
|
The Frobenius norm grows monotonically across the entire DEGRADED
|
||||||
|
window — the AC-9 honest-covariance invariant is upheld by the
|
||||||
|
formula's structure, not by a floor clamp.
|
||||||
@@ -0,0 +1,242 @@
|
|||||||
|
# Code Review Report
|
||||||
|
|
||||||
|
**Batch**: 54 (AZ-334 — C1 KLT/RANSAC Strategy)
|
||||||
|
**Date**: 2026-05-14
|
||||||
|
**Verdict**: PASS_WITH_WARNINGS
|
||||||
|
|
||||||
|
## Scope
|
||||||
|
|
||||||
|
Single-task batch implementing `KltRansacStrategy`, the mandatory
|
||||||
|
simple-baseline `VioStrategy` that satisfies the ADR-002 engine rule
|
||||||
|
(every component MUST ship a simple-baseline strategy alongside its
|
||||||
|
production-default). Pure-Python over OpenCV's `cv2.goodFeaturesToTrack`
|
||||||
|
/ `cv2.calcOpticalFlowPyrLK` / `cv2.findEssentialMat` / `cv2.recoverPose`
|
||||||
|
path; no C++/pybind11 native binding by design — Tier-0 workstation can
|
||||||
|
run the strategy with `pip install opencv-python` only.
|
||||||
|
|
||||||
|
### Changed files
|
||||||
|
|
||||||
|
- `src/gps_denied_onboard/components/c1_vio/klt_ransac.py` — new
|
||||||
|
Python facade (~770 lines, including module docstring + AC mapping
|
||||||
|
+ risk-mitigation notes).
|
||||||
|
- `src/gps_denied_onboard/components/c1_vio/config.py` — add
|
||||||
|
`KltRansacConfig` dataclass + `klt_ransac` field on `C1VioConfig`;
|
||||||
|
`__all__` updated; `KNOWN_STRATEGIES` already had `klt_ransac`.
|
||||||
|
- `src/gps_denied_onboard/components/c1_vio/__init__.py` — export
|
||||||
|
`KltRansacConfig`.
|
||||||
|
- `cpp/klt_ransac/CMakeLists.txt` — replace placeholder message
|
||||||
|
with a deliberate "pure-Python; no native target" explanation so
|
||||||
|
the build graph stays symmetric with `cpp/okvis2/` and
|
||||||
|
`cpp/vins_mono/` while the `BUILD_KLT_RANSAC=ON` flag only gates
|
||||||
|
the Python module import at the AZ-331 composition-root factory.
|
||||||
|
- `tests/unit/c1_vio/test_klt_ransac_strategy.py` — new test module
|
||||||
|
covering AC-1..AC-11 + NFR-perf (~990 lines, 25 tests; 23 pass + 2
|
||||||
|
tier-2 skipped on dev/CI runners).
|
||||||
|
- `tests/unit/c1_vio/test_protocol_conformance.py` — introduce the
|
||||||
|
`_STRATEGIES_WITHOUT_NATIVE_BINDING` category and route
|
||||||
|
`klt_ransac` through it inside `test_ac5_build_vio_strategy_flag_on_but_module_missing`
|
||||||
|
so the parametrised factory test correctly handles the pure-Python
|
||||||
|
shape (no native binding to fail on).
|
||||||
|
|
||||||
|
## Phase 2 — Spec Compliance
|
||||||
|
|
||||||
|
| AC | Test | Verified |
|
||||||
|
|-------|----------------------------------------------------------------------------|----------|
|
||||||
|
| AC-1 | `test_ac1_current_strategy_label_returns_klt_ransac` + | ✓ |
|
||||||
|
| | `test_ac1_constructor_rejects_mismatched_strategy_label` | |
|
||||||
|
| AC-2 | `test_ac2_first_frame_emits_init_state_with_identity_pose` | ✓ |
|
||||||
|
| AC-3 | `test_ac3_steady_state_frame_emits_pose_and_spd_covariance` | ✓ |
|
||||||
|
| AC-4 | `test_ac4_cv2_error_in_find_essential_mat_rewrapped_to_vio_fatal_error` + | ✓ |
|
||||||
|
| | `test_ac4_cv2_error_in_recover_pose_rewrapped_to_vio_fatal_error` | |
|
||||||
|
| AC-5 | `test_ac5_reset_to_warm_start_clears_feature_buffer_and_seeds_bias` + | ✓ |
|
||||||
|
| | `test_ac5_reset_to_warm_start_idempotent_across_consecutive_calls` + | |
|
||||||
|
| | `test_ac5_reset_to_warm_start_rejects_non_pose3_hint` | |
|
||||||
|
| AC-6 | `test_ac6_low_inlier_count_emits_degraded_with_monotonic_covariance` | ✓ |
|
||||||
|
| AC-7 | `test_ac7_sustained_pose_recovery_failure_raises_vio_fatal_error` | ✓ |
|
||||||
|
| AC-8 | `test_ac8_strategy_module_not_imported_at_package_load` + | ✓ |
|
||||||
|
| | `test_protocol_conformance.py::test_ac5_build_vio_strategy_flag_*` | |
|
||||||
|
| AC-9 | `test_ac9_honest_covariance_monotonic_during_degraded` (tier2) | ✓ |
|
||||||
|
| AC-10 | `test_ac10_fdr_vio_health_emitted_per_transition` | ✓ |
|
||||||
|
| AC-11 | `test_ac11_source_has_no_camera_id_literals` + | ✓ |
|
||||||
|
| | `test_ac11_strategy_handles_two_distinct_calibrations` | |
|
||||||
|
| NFR-perf | `test_nfr_perf_process_frame_records_p95` (tier2) | ✓ |
|
||||||
|
|
||||||
|
All 11 ACs + NFR-perf mapped to passing tests. Test suite reports
|
||||||
|
25 tests, 23 pass + 2 tier2 skipped on the standard dev/CI runner;
|
||||||
|
all 25 pass under `GPS_DENIED_TIER=2`. Adjacent regression:
|
||||||
|
`tests/unit/c1_vio/` reports 95 passed + 6 skipped (6 = the 2
|
||||||
|
KLT-tier2 + 2 OKVIS2-tier2 + 2 VINS-Mono-tier2 tests); config-loader
|
||||||
|
and compose-root suites green (17 passed).
|
||||||
|
|
||||||
|
## Phase 3 — Code Quality
|
||||||
|
|
||||||
|
- **SOLID**: `KltRansacStrategy` has a single responsibility (Python
|
||||||
|
facade over OpenCV's KLT/RANSAC path). Constructor injection per
|
||||||
|
ADR-009 — `Config` + `FdrClient` enter explicitly; `Clock` is
|
||||||
|
optional with a `WallClock` default; the AZ-276 `ImuPreintegrator`
|
||||||
|
is constructed lazily on the first `process_frame` call (it
|
||||||
|
requires the per-call `CameraCalibration` which is not available
|
||||||
|
at constructor time — matches the existing factory pattern across
|
||||||
|
OKVIS2 / VINS-Mono).
|
||||||
|
- **Error handling**: error envelope closed at the `VioError` family;
|
||||||
|
every OpenCV `cv2.error` is caught at each call site and rewrapped
|
||||||
|
into `VioFatalError` with `__cause__` chaining (AC-4). Pose-recovery
|
||||||
|
failures route through `_pose_recovery_failed` which raises
|
||||||
|
`VioInitializingError` until `lost_frame_threshold` is exhausted,
|
||||||
|
then escalates to `VioFatalError` (AC-7). RansacFilterError +
|
||||||
|
ImuPreintegrationError are also caught and rewrapped.
|
||||||
|
- **Naming**: matches the OKVIS2 / VINS-Mono facade naming exactly
|
||||||
|
(intentional — IT-12 harness substitutability).
|
||||||
|
- **Complexity**: `process_frame` is ~120 lines; the dominant cost is
|
||||||
|
the explicit step-numbered ladder (IMU push → grayscale → first-frame
|
||||||
|
branch → KLT track → RANSAC filter → essential-matrix → pose recover
|
||||||
|
→ covariance → VioOutput build → state classify → re-seed features).
|
||||||
|
Splitting further would obscure the linear flow that maps 1:1 onto
|
||||||
|
the task spec's "Outcome" numbered list.
|
||||||
|
- **DRY**: structural duplication with OKVIS2 / VINS-Mono facades —
|
||||||
|
see F1 below; deliberately deferred to the post-batch-54 hygiene
|
||||||
|
PBI (now scheduled by the next cumulative review, batches 52-54).
|
||||||
|
- **Test quality**: each AC has a behaviourally-meaningful assertion
|
||||||
|
(covariance SPD, frame_id echoed, transition states ordered,
|
||||||
|
monotonic covariance growth during DEGRADED, etc.). No "did not
|
||||||
|
throw" placeholder tests. AC-3 / AC-6 / AC-9 / AC-10 / AC-11 /
|
||||||
|
NFR-perf monkeypatch `cv2.findEssentialMat` + `cv2.recoverPose`
|
||||||
|
+ `RansacFilter.filter_correspondences` to deterministic values so
|
||||||
|
the unit suite exercises the FACADE's state machine without
|
||||||
|
depending on real OpenCV geometry on synthetic correspondences;
|
||||||
|
real-geometry validation lives in C1-IT-12 (Jetson Tier-2 fixture).
|
||||||
|
- **Dead code**: none. `_drain_into_list` removed from the test
|
||||||
|
helper after the simpler `_drain` was introduced.
|
||||||
|
|
||||||
|
## Phase 4 — Security Quick-Scan
|
||||||
|
|
||||||
|
- No SQL / command injection paths. No `subprocess(shell=True)`,
|
||||||
|
`eval`, `exec`.
|
||||||
|
- No hardcoded secrets, API keys, or credentials.
|
||||||
|
- Input validation: `_intrinsics_3x3` rejects non-3x3 K with
|
||||||
|
`VioFatalError`; `_grayscale` rejects unsupported image shapes;
|
||||||
|
`KltRansacConfig.__post_init__` validates every knob range
|
||||||
|
(max_corners ≥ 4, klt_window_size_px odd ≥ 3, klt_pyramid_levels
|
||||||
|
≥ 1, min_features_for_pose ≥ 5, ransac_inlier_ratio in (0, 1],
|
||||||
|
essential_matrix_ransac_threshold_px > 0).
|
||||||
|
- Sensitive data: per-frame DEBUG log defaults OFF
|
||||||
|
(`KltRansacConfig.per_frame_debug_log = False`) — matches
|
||||||
|
`description.md` § 9 logging hygiene.
|
||||||
|
|
||||||
|
PASS.
|
||||||
|
|
||||||
|
## Phase 5 — Performance Scan
|
||||||
|
|
||||||
|
- Hot path: `process_frame`. IMU push loop is
|
||||||
|
`O(samples_per_window)` — unavoidable. KLT track + RANSAC are
|
||||||
|
multi-threaded internally by OpenCV; bound at 30 % of one core
|
||||||
|
per ADR-002 budget partition.
|
||||||
|
- No N+1, no unbounded buffers (FdrClient capacity bounded by
|
||||||
|
AZ-273 ringbuf; the strategy keeps a single
|
||||||
|
`_prev_features` numpy array sized at `max_corners`).
|
||||||
|
- Covariance estimator: `_estimate_covariance` does one
|
||||||
|
`np.eye(6) * scalar` — O(1).
|
||||||
|
- AC-9 honest-covariance: the residual_var / DOF formula has no
|
||||||
|
client-side floor; cov Frobenius grows monotonically as
|
||||||
|
inlier_count drops (verified in tier-2 test).
|
||||||
|
|
||||||
|
PASS.
|
||||||
|
|
||||||
|
## Phase 6 — Cross-Task Consistency
|
||||||
|
|
||||||
|
N/A — single-task batch. Cross-task consistency with AZ-332 +
|
||||||
|
AZ-333 is tracked in the next cumulative review (batches 52-54),
|
||||||
|
which will see all three strategy facades and the duplication
|
||||||
|
finding (F1) at the same time.
|
||||||
|
|
||||||
|
## Phase 7 — Architecture Compliance
|
||||||
|
|
||||||
|
- **Layer direction**: `klt_ransac.py` imports from `_types.nav`,
|
||||||
|
`_types.calibration` (TYPE_CHECKING only), `clock.wall_clock`,
|
||||||
|
`components.c1_vio.config` (TYPE_CHECKING only),
|
||||||
|
`components.c1_vio.errors`, `fdr_client`, `helpers.imu_preintegrator`,
|
||||||
|
`helpers.ransac_filter`, `logging` — all L1/L2 substrate per the
|
||||||
|
c1 layering. PASS.
|
||||||
|
- **Public API respect**: `KltRansacConfig` exported through
|
||||||
|
`c1_vio/__init__.py`; `KltRansacStrategy` deliberately NOT exported
|
||||||
|
(lazy import only via `runtime_root.vio_factory`) — matches the
|
||||||
|
Risk-2/Risk-3 pattern from OKVIS2 and VINS-Mono. PASS.
|
||||||
|
- **No new cyclic dependencies**: introduced module is a leaf — no
|
||||||
|
back-edges to its own importers.
|
||||||
|
- **Native binding location**: NONE by design. `cpp/klt_ransac/`
|
||||||
|
carries a CMakeLists that returns a STATUS message documenting
|
||||||
|
the absence of native target; the directory is preserved for
|
||||||
|
build-graph symmetry with `cpp/okvis2/` and `cpp/vins_mono/`.
|
||||||
|
- **Build flag respect**: `BUILD_KLT_RANSAC=OFF` keeps the
|
||||||
|
composition-root factory from importing the strategy module; the
|
||||||
|
AZ-331 factory raises `StrategyNotAvailableError` before any
|
||||||
|
import — Risk-3 mitigation intact for operator-tooling binaries
|
||||||
|
(which do not need any VIO at all).
|
||||||
|
- **AC-11 camera agnostic**: source CI-grep gate verifies no
|
||||||
|
`adti20` / `adti26` literals in executable code (docstrings are
|
||||||
|
excluded via AST-aware stripping in the test). PASS.
|
||||||
|
|
||||||
|
PASS.
|
||||||
|
|
||||||
|
## Findings
|
||||||
|
|
||||||
|
| # | Severity | Category | File:Line | Title |
|
||||||
|
|---|----------|----------|-----------|-------|
|
||||||
|
| 1 | Low | Maintainability | `src/gps_denied_onboard/components/c1_vio/klt_ransac.py` | Structural duplication with `okvis2.py` / `vins_mono.py` — now scheduled for post-batch-54 hygiene PBI via cumulative review |
|
||||||
|
| 2 | Low | Maintainability | `tests/unit/c1_vio/test_klt_ransac_strategy.py` | `_patch_pose_recovery` helper is bespoke per-strategy; the same patching pattern could plausibly be shared with OKVIS2 / VINS-Mono fake-binding fixtures |
|
||||||
|
|
||||||
|
### Finding details
|
||||||
|
|
||||||
|
**F1: Structural duplication of strategy facade** (Low / Maintainability)
|
||||||
|
|
||||||
|
- Location: `src/gps_denied_onboard/components/c1_vio/klt_ransac.py`
|
||||||
|
vs `src/gps_denied_onboard/components/c1_vio/okvis2.py` /
|
||||||
|
`vins_mono.py`
|
||||||
|
- Description: The new `KltRansacStrategy` mirrors `Okvis2Strategy`
|
||||||
|
+ `VinsMonoStrategy` ~70 % verbatim on the orchestration spine —
|
||||||
|
`_classify_state`, `_tick_lost`, `_emit_transition`,
|
||||||
|
`_bias_norm`, `_now_iso`, `_se3_from_4x4`, the constructor strategy-
|
||||||
|
label guard, and the FDR record-emit shape are byte-equivalent
|
||||||
|
modulo strategy-label constants. The geometry-specific pipeline
|
||||||
|
(KLT seed/track, RANSAC filter, findEssentialMat, recoverPose,
|
||||||
|
residual-scatter covariance) IS unique to this strategy and lives
|
||||||
|
in its own module — that boundary is correct. The shared
|
||||||
|
orchestration spine is the consolidation target tracked by the
|
||||||
|
hygiene PBI deferred since batch 53; the cumulative review
|
||||||
|
scheduled for batches 52-54 (next trigger) will formally raise the
|
||||||
|
PBI now that all three strategy shapes are visible.
|
||||||
|
- Suggestion: defer (one more time) to the cumulative review for
|
||||||
|
batches 52-54 — the right factoring is now visible (template-
|
||||||
|
method base class for the orchestration spine + per-strategy
|
||||||
|
geometry hook). Do NOT create the PBI ad-hoc here; let the
|
||||||
|
cumulative review own the cross-batch refactor scope.
|
||||||
|
- Task: AZ-334
|
||||||
|
|
||||||
|
**F2: Test patching helper could be shared** (Low / Maintainability)
|
||||||
|
|
||||||
|
- Location: `tests/unit/c1_vio/test_klt_ransac_strategy.py`
|
||||||
|
(`_patch_pose_recovery`) vs `tests/unit/c1_vio/conftest.py`
|
||||||
|
(`FakeOkvis2Backend` / `FakeVinsMonoBackend`)
|
||||||
|
- Description: KLT/RANSAC uses real OpenCV bindings (no fake-binding
|
||||||
|
fixture) so the existing conftest fakes don't apply directly. The
|
||||||
|
per-test helper `_patch_pose_recovery` does the equivalent job of
|
||||||
|
forcing a deterministic-success path. This is a different
|
||||||
|
abstraction shape from the conftest fakes but lives at the same
|
||||||
|
layer; consolidating with the post-batch-54 hygiene PBI would let
|
||||||
|
all three strategies share a single per-strategy "ScriptedSuccess"
|
||||||
|
fixture surface.
|
||||||
|
- Suggestion: same as F1 — let the cumulative review's hygiene PBI
|
||||||
|
own the cross-cutting test-fixture refactor.
|
||||||
|
- Task: AZ-334
|
||||||
|
|
||||||
|
## Verdict
|
||||||
|
|
||||||
|
**PASS_WITH_WARNINGS** — two Low-severity duplication findings, both
|
||||||
|
intentionally deferred and now formally in scope for the next
|
||||||
|
cumulative review (batches 52-54). No Critical, High, or Medium
|
||||||
|
findings. All 11 ACs + NFR-perf covered with passing tests.
|
||||||
|
|
||||||
|
Pre-existing environment-dependent perf flake noted in batch 53
|
||||||
|
(`tests/unit/c12_operator_orchestrator/test_cli_console_script.py::test_cold_start_under_500ms_p99`)
|
||||||
|
is still environmental and untouched by this batch — reported, not
|
||||||
|
blocking.
|
||||||
@@ -12,5 +12,6 @@ sub_step:
|
|||||||
retry_count: 0
|
retry_count: 0
|
||||||
cycle: 1
|
cycle: 1
|
||||||
tracker: jira
|
tracker: jira
|
||||||
last_completed_batch: 53
|
last_completed_batch: 54
|
||||||
last_cumulative_review: batches_49-51
|
last_cumulative_review: batches_49-51
|
||||||
|
current_batch: 55
|
||||||
|
|||||||
@@ -1,4 +1,16 @@
|
|||||||
if(NOT BUILD_KLT_RANSAC)
|
if(NOT BUILD_KLT_RANSAC)
|
||||||
return()
|
return()
|
||||||
endif()
|
endif()
|
||||||
message(STATUS "[klt_ransac] Placeholder; concrete sources land with AZ-334.")
|
|
||||||
|
# AZ-334 — KLT/RANSAC strategy is PURE PYTHON over OpenCV's Python
|
||||||
|
# bindings (cv2.calcOpticalFlowPyrLK / goodFeaturesToTrack /
|
||||||
|
# findEssentialMat / recoverPose). There is no native binding under
|
||||||
|
# this strategy by design — the simple-baseline path must remain
|
||||||
|
# dependency-light (Tier-0 workstation can run KLT/RANSAC with only
|
||||||
|
# `pip install opencv-python`; no OKVIS2 / VINS-Mono native libs
|
||||||
|
# required). This directory + CMake target is preserved for build-
|
||||||
|
# graph symmetry with cpp/okvis2/ and cpp/vins_mono/; the BUILD_KLT_RANSAC
|
||||||
|
# flag still gates the Python module import at the AZ-331 composition
|
||||||
|
# root factory (`runtime_root/vio_factory.py`).
|
||||||
|
message(STATUS "[klt_ransac] AZ-334 — pure-Python strategy; no native target. "
|
||||||
|
"BUILD_KLT_RANSAC=ON gates the Python module import only.")
|
||||||
|
|||||||
@@ -27,6 +27,7 @@ from gps_denied_onboard._types.nav import (
|
|||||||
)
|
)
|
||||||
from gps_denied_onboard.components.c1_vio.config import (
|
from gps_denied_onboard.components.c1_vio.config import (
|
||||||
C1VioConfig,
|
C1VioConfig,
|
||||||
|
KltRansacConfig,
|
||||||
Okvis2Config,
|
Okvis2Config,
|
||||||
VinsMonoConfig,
|
VinsMonoConfig,
|
||||||
)
|
)
|
||||||
@@ -44,6 +45,7 @@ register_component_block("c1_vio", C1VioConfig)
|
|||||||
__all__ = [
|
__all__ = [
|
||||||
"C1VioConfig",
|
"C1VioConfig",
|
||||||
"FeatureQuality",
|
"FeatureQuality",
|
||||||
|
"KltRansacConfig",
|
||||||
"Okvis2Config",
|
"Okvis2Config",
|
||||||
"VinsMonoConfig",
|
"VinsMonoConfig",
|
||||||
"VioDegradedError",
|
"VioDegradedError",
|
||||||
|
|||||||
@@ -1,4 +1,4 @@
|
|||||||
"""C1 VIO strategy config block (AZ-331 + AZ-332 + AZ-333).
|
"""C1 VIO strategy config block (AZ-331 + AZ-332 + AZ-333 + AZ-334).
|
||||||
|
|
||||||
Registered into ``config.components['c1_vio']`` by the package
|
Registered into ``config.components['c1_vio']`` by the package
|
||||||
``__init__.py``. The composition-root factory
|
``__init__.py``. The composition-root factory
|
||||||
@@ -17,6 +17,13 @@ research-only VINS-Mono backend (sliding-window size, feature tracker
|
|||||||
thresholds, marginalisation strategy, max optimisation iterations,
|
thresholds, marginalisation strategy, max optimisation iterations,
|
||||||
degraded-feature threshold, per-frame debug log). Only consulted when
|
degraded-feature threshold, per-frame debug log). Only consulted when
|
||||||
``strategy == "vins_mono"``.
|
``strategy == "vins_mono"``.
|
||||||
|
|
||||||
|
AZ-334 extends with a sibling :class:`KltRansacConfig` for the
|
||||||
|
mandatory simple-baseline pure-Python OpenCV KLT/RANSAC backend
|
||||||
|
(max corners, KLT pyramid levels, KLT window size, essential-matrix
|
||||||
|
RANSAC threshold, RANSAC inlier ratio for the AZ-282 helper stage,
|
||||||
|
min features for pose, per-frame debug log). Only consulted when
|
||||||
|
``strategy == "klt_ransac"``.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
@@ -29,6 +36,7 @@ from gps_denied_onboard.config.schema import ConfigError
|
|||||||
__all__ = [
|
__all__ = [
|
||||||
"KNOWN_STRATEGIES",
|
"KNOWN_STRATEGIES",
|
||||||
"C1VioConfig",
|
"C1VioConfig",
|
||||||
|
"KltRansacConfig",
|
||||||
"Okvis2Config",
|
"Okvis2Config",
|
||||||
"VinsMonoConfig",
|
"VinsMonoConfig",
|
||||||
]
|
]
|
||||||
@@ -174,6 +182,83 @@ class VinsMonoConfig:
|
|||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class KltRansacConfig:
|
||||||
|
"""KLT/RANSAC-specific knobs (AZ-334; mandatory simple-baseline).
|
||||||
|
|
||||||
|
``max_corners`` is the per-frame upper bound on features extracted
|
||||||
|
by ``cv2.goodFeaturesToTrack``; default 200 (OpenCV documentation
|
||||||
|
suggests 100-500 for visual-odometry use).
|
||||||
|
|
||||||
|
``klt_window_size_px`` is the per-level search window edge length
|
||||||
|
(square) passed to ``cv2.calcOpticalFlowPyrLK``; default 21 (the
|
||||||
|
OpenCV cookbook default for moderately-fast UAV motion).
|
||||||
|
|
||||||
|
``klt_pyramid_levels`` is the number of pyramid levels in the
|
||||||
|
pyramidal Lucas-Kanade tracker; default 3 (covers ~8x scale span
|
||||||
|
per level so 3 levels handle typical UAV inter-frame motion).
|
||||||
|
|
||||||
|
``min_features_for_pose`` is the inlier-count floor below which
|
||||||
|
``health_snapshot`` reports DEGRADED; pose recovery is still
|
||||||
|
attempted but the per-frame covariance is inflated. Default 30,
|
||||||
|
matching ``Okvis2Config.degraded_feature_threshold`` so the
|
||||||
|
cross-strategy DEGRADED gate is consistent.
|
||||||
|
|
||||||
|
``ransac_inlier_ratio`` is the inlier ratio threshold the
|
||||||
|
AZ-282 :class:`RansacFilter` stage uses to reject correspondences
|
||||||
|
BEFORE the essential-matrix recovery stage; default 0.5. Surfaced
|
||||||
|
here because the helper itself is stateless.
|
||||||
|
|
||||||
|
``essential_matrix_ransac_threshold_px`` is the per-pixel
|
||||||
|
reprojection-error threshold passed to ``cv2.findEssentialMat``'s
|
||||||
|
internal RANSAC; default 1.0 px in normalised image coordinates
|
||||||
|
(OpenCV's documented default for forward-looking cameras).
|
||||||
|
|
||||||
|
``per_frame_debug_log`` enables a DEBUG log line per
|
||||||
|
``process_frame`` — OFF by default (would flood at 3 Hz steady-state).
|
||||||
|
"""
|
||||||
|
|
||||||
|
max_corners: int = 200
|
||||||
|
klt_window_size_px: int = 21
|
||||||
|
klt_pyramid_levels: int = 3
|
||||||
|
min_features_for_pose: int = 30
|
||||||
|
ransac_inlier_ratio: float = 0.5
|
||||||
|
essential_matrix_ransac_threshold_px: float = 1.0
|
||||||
|
per_frame_debug_log: bool = False
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
if self.max_corners < 4:
|
||||||
|
raise ConfigError(
|
||||||
|
"KltRansacConfig.max_corners must be >= 4 (essential matrix "
|
||||||
|
f"requires >=5 correspondences); got {self.max_corners}"
|
||||||
|
)
|
||||||
|
if self.klt_window_size_px < 3 or self.klt_window_size_px % 2 == 0:
|
||||||
|
raise ConfigError(
|
||||||
|
"KltRansacConfig.klt_window_size_px must be an odd integer "
|
||||||
|
f">= 3; got {self.klt_window_size_px}"
|
||||||
|
)
|
||||||
|
if self.klt_pyramid_levels < 1:
|
||||||
|
raise ConfigError(
|
||||||
|
"KltRansacConfig.klt_pyramid_levels must be >= 1; "
|
||||||
|
f"got {self.klt_pyramid_levels}"
|
||||||
|
)
|
||||||
|
if self.min_features_for_pose < 5:
|
||||||
|
raise ConfigError(
|
||||||
|
"KltRansacConfig.min_features_for_pose must be >= 5 (essential "
|
||||||
|
f"matrix DOF floor); got {self.min_features_for_pose}"
|
||||||
|
)
|
||||||
|
if not (0.0 < self.ransac_inlier_ratio <= 1.0):
|
||||||
|
raise ConfigError(
|
||||||
|
"KltRansacConfig.ransac_inlier_ratio must be in (0.0, 1.0]; "
|
||||||
|
f"got {self.ransac_inlier_ratio}"
|
||||||
|
)
|
||||||
|
if self.essential_matrix_ransac_threshold_px <= 0.0:
|
||||||
|
raise ConfigError(
|
||||||
|
"KltRansacConfig.essential_matrix_ransac_threshold_px must be "
|
||||||
|
f"> 0; got {self.essential_matrix_ransac_threshold_px}"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class C1VioConfig:
|
class C1VioConfig:
|
||||||
"""Per-component config for C1 VIO.
|
"""Per-component config for C1 VIO.
|
||||||
@@ -195,6 +280,9 @@ class C1VioConfig:
|
|||||||
|
|
||||||
``vins_mono`` carries VINS-Mono-specific knobs (AZ-333); consulted
|
``vins_mono`` carries VINS-Mono-specific knobs (AZ-333); consulted
|
||||||
only when ``strategy == "vins_mono"``.
|
only when ``strategy == "vins_mono"``.
|
||||||
|
|
||||||
|
``klt_ransac`` carries KLT/RANSAC-specific knobs (AZ-334);
|
||||||
|
consulted only when ``strategy == "klt_ransac"``.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
strategy: str = "klt_ransac"
|
strategy: str = "klt_ransac"
|
||||||
@@ -202,6 +290,7 @@ class C1VioConfig:
|
|||||||
warm_start_max_frames: int = 5
|
warm_start_max_frames: int = 5
|
||||||
okvis2: Okvis2Config = field(default_factory=Okvis2Config)
|
okvis2: Okvis2Config = field(default_factory=Okvis2Config)
|
||||||
vins_mono: VinsMonoConfig = field(default_factory=VinsMonoConfig)
|
vins_mono: VinsMonoConfig = field(default_factory=VinsMonoConfig)
|
||||||
|
klt_ransac: KltRansacConfig = field(default_factory=KltRansacConfig)
|
||||||
|
|
||||||
def __post_init__(self) -> None:
|
def __post_init__(self) -> None:
|
||||||
if self.strategy not in KNOWN_STRATEGIES:
|
if self.strategy not in KNOWN_STRATEGIES:
|
||||||
|
|||||||
@@ -0,0 +1,769 @@
|
|||||||
|
"""`KltRansacStrategy` — mandatory simple-baseline C1 VIO (AZ-334).
|
||||||
|
|
||||||
|
Pure-Python facade over OpenCV's pyramidal Lucas-Kanade optical-flow +
|
||||||
|
essential-matrix RANSAC path. The ADR-002 engine-rule mandatory
|
||||||
|
baseline: every airborne binary MUST link a simple-baseline strategy
|
||||||
|
alongside the production-default. KLT/RANSAC is the lowest-complexity
|
||||||
|
strategy in E-C1 by code volume — no C++/pybind11, no native binding —
|
||||||
|
so a Tier-0 workstation can run it with only ``pip install opencv-python``
|
||||||
|
and the AZ-331 factory's ``BUILD_KLT_RANSAC=ON`` gate.
|
||||||
|
|
||||||
|
Conforms to the AZ-331 :class:`VioStrategy` Protocol; consumes the
|
||||||
|
runtime ``Config`` + an :class:`FdrClient`; constructs its other
|
||||||
|
dependencies (logger, KLT/RANSAC sub-config, IMU preintegrator) from
|
||||||
|
``config`` so the strategy class matches the composition-root factory
|
||||||
|
shape::
|
||||||
|
|
||||||
|
strategy_cls(config: Config, *, fdr_client: FdrClient)
|
||||||
|
|
||||||
|
This mirrors :class:`Okvis2Strategy` (AZ-332) + :class:`VinsMonoStrategy`
|
||||||
|
(AZ-333) deliberately: the AZ-331 factory produces all three via the
|
||||||
|
same ``(config, *, fdr_client)`` shape and the IT-12 comparative-study
|
||||||
|
harness expects them to be drop-in substitutable. The structural
|
||||||
|
duplication of the constructor / state machine / error-rewrap ladder
|
||||||
|
is tracked for the post-AZ-334 hygiene PBI (Batch 53 review F1).
|
||||||
|
|
||||||
|
AC mapping (see ``_docs/02_tasks/done/AZ-334_c1_klt_ransac_strategy.md``):
|
||||||
|
|
||||||
|
- AC-1 : :meth:`current_strategy_label` returns ``"klt_ransac"``.
|
||||||
|
- AC-2 : First :meth:`process_frame` emits :class:`VioOutput` with
|
||||||
|
identity relative pose, conservative INIT-state covariance, and
|
||||||
|
``health_snapshot().state == INIT``.
|
||||||
|
- AC-3 : Steady-state :meth:`process_frame` emits :class:`VioOutput`
|
||||||
|
with non-identity relative pose, SPD covariance, ``mre_px > 0``.
|
||||||
|
- AC-4 : ``cv2.error`` from :func:`cv2.findEssentialMat` /
|
||||||
|
:func:`cv2.recoverPose` rewraps into :class:`VioFatalError` with a
|
||||||
|
``__cause__`` chain; no raw ``cv2.error`` leaks.
|
||||||
|
- AC-5 : :meth:`reset_to_warm_start` clears the feature buffer +
|
||||||
|
re-seeds the IMU bias via the AZ-276 preintegrator's
|
||||||
|
:meth:`reset_with_bias`; idempotent across consecutive calls.
|
||||||
|
- AC-6 : Inlier loss → :class:`VioState.DEGRADED` + monotonically
|
||||||
|
growing covariance Frobenius norm; :class:`VioOutput` IS emitted
|
||||||
|
(not raised).
|
||||||
|
- AC-7 : ``lost_frame_threshold`` consecutive failed-pose frames →
|
||||||
|
:class:`VioFatalError`; ``health_snapshot().state == LOST``.
|
||||||
|
- AC-8 : ``BUILD_KLT_RANSAC=OFF`` does not import this module —
|
||||||
|
enforced by AZ-331's factory in
|
||||||
|
:mod:`gps_denied_onboard.runtime_root.vio_factory`;
|
||||||
|
``StrategyNotAvailableError`` is the surfaced error.
|
||||||
|
- AC-9 : Honest covariance — no shrinkage during DEGRADED; the
|
||||||
|
per-frame covariance is the residual-scatter formula divided by
|
||||||
|
the inlier-DOF (``N_inliers - 5``) with no client-side floor or
|
||||||
|
smoother.
|
||||||
|
- AC-10: Exactly one ``vio.health`` FDR record per state transition;
|
||||||
|
no spam on steady-state.
|
||||||
|
- AC-11: Camera-agnostic source — no ``adti20`` / ``adti26`` literals;
|
||||||
|
the per-call :class:`CameraCalibration` argument carries intrinsics.
|
||||||
|
|
||||||
|
Risk mitigations (see task spec for full text):
|
||||||
|
|
||||||
|
- *Risk 1 — residual-scatter under-reports during high-overlap straight
|
||||||
|
flight*: documented; the C1-IT-12 comparative-study report cross-
|
||||||
|
validates against OKVIS2. No code mitigation in this strategy.
|
||||||
|
- *Risk 2 — KLT track loss on first frame*: AC-2 handles INIT state +
|
||||||
|
FDR record on transition.
|
||||||
|
- *Risk 3 — RANSAC threshold sensitivity*: surfaced via
|
||||||
|
``KltRansacConfig.essential_matrix_ransac_threshold_px``; the
|
||||||
|
AZ-282 :class:`RansacFilter` pre-filter runs at the
|
||||||
|
``essential_matrix_ransac_threshold_px`` boundary, then
|
||||||
|
:func:`cv2.findEssentialMat`'s internal RANSAC runs again on the
|
||||||
|
surviving inlier set — two stages with separate determinism gates.
|
||||||
|
- *Risk 4 — RESTRICT-UAV-3 sharp turns*: DEGRADED reported immediately;
|
||||||
|
recovery is F6 satellite re-localisation (E-C2 / E-C3 / E-C4 path).
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
from datetime import datetime, timezone
|
||||||
|
from typing import TYPE_CHECKING, Any, Final, Literal
|
||||||
|
|
||||||
|
import cv2
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from gps_denied_onboard._types.nav import (
|
||||||
|
FeatureQuality,
|
||||||
|
ImuBias,
|
||||||
|
VioHealth,
|
||||||
|
VioOutput,
|
||||||
|
VioState,
|
||||||
|
)
|
||||||
|
from gps_denied_onboard.clock.wall_clock import WallClock
|
||||||
|
from gps_denied_onboard.components.c1_vio.errors import (
|
||||||
|
VioFatalError,
|
||||||
|
VioInitializingError,
|
||||||
|
)
|
||||||
|
from gps_denied_onboard.fdr_client.records import CURRENT_SCHEMA_VERSION, FdrRecord
|
||||||
|
from gps_denied_onboard.helpers.imu_preintegrator import (
|
||||||
|
ImuPreintegrationError,
|
||||||
|
ImuPreintegrator,
|
||||||
|
make_imu_preintegrator,
|
||||||
|
)
|
||||||
|
from gps_denied_onboard.helpers.ransac_filter import RansacFilter, RansacFilterError
|
||||||
|
from gps_denied_onboard.logging import get_logger
|
||||||
|
|
||||||
|
if TYPE_CHECKING:
|
||||||
|
import numpy.typing as npt
|
||||||
|
|
||||||
|
from gps_denied_onboard._types.calibration import CameraCalibration
|
||||||
|
from gps_denied_onboard._types.nav import (
|
||||||
|
ImuWindow,
|
||||||
|
NavCameraFrame,
|
||||||
|
WarmStartPose,
|
||||||
|
)
|
||||||
|
from gps_denied_onboard.clock import Clock
|
||||||
|
from gps_denied_onboard.components.c1_vio.config import KltRansacConfig
|
||||||
|
from gps_denied_onboard.config import Config
|
||||||
|
from gps_denied_onboard.fdr_client.client import FdrClient
|
||||||
|
|
||||||
|
|
||||||
|
__all__ = ["KltRansacStrategy"]
|
||||||
|
|
||||||
|
|
||||||
|
_STRATEGY_LABEL: Final[Literal["klt_ransac"]] = "klt_ransac"
|
||||||
|
_PRODUCER_ID: Final[str] = "c1_vio.klt_ransac"
|
||||||
|
_LOGGER_COMPONENT: Final[str] = "c1_vio.klt_ransac"
|
||||||
|
# Essential matrix has 5 degrees of freedom (E in R^{3x3} with rank-2 +
|
||||||
|
# scale ambiguity); residual-scatter covariance DOF = N_inliers - 5.
|
||||||
|
_ESSENTIAL_MATRIX_DOF: Final[int] = 5
|
||||||
|
# INIT-state conservative covariance scalar applied uniformly to the
|
||||||
|
# 6x6 identity. Larger than typical TRACKING-state covariance so C5
|
||||||
|
# fusion treats the first-frame pose as effectively un-informed
|
||||||
|
# (relative pose is identity anyway). Documented limit, not derived.
|
||||||
|
_INIT_STATE_COVARIANCE_SCALAR: Final[float] = 10.0
|
||||||
|
|
||||||
|
|
||||||
|
def _now_iso() -> str:
|
||||||
|
return datetime.now(timezone.utc).isoformat()
|
||||||
|
|
||||||
|
|
||||||
|
def _bias_norm(bias: ImuBias) -> float:
|
||||||
|
"""L2 norm of the concatenated 6-vector ``(accel || gyro)``."""
|
||||||
|
accel = np.asarray(bias.accel_bias, dtype=np.float64)
|
||||||
|
gyro = np.asarray(bias.gyro_bias, dtype=np.float64)
|
||||||
|
return float(math.sqrt(float(np.dot(accel, accel) + np.dot(gyro, gyro))))
|
||||||
|
|
||||||
|
|
||||||
|
def _se3_from_4x4(matrix: npt.NDArray[Any]) -> Any:
|
||||||
|
"""Build a ``gtsam.Pose3`` from a 4x4 row-major matrix.
|
||||||
|
|
||||||
|
Imported lazily so this module can be imported without gtsam in
|
||||||
|
headless tooling paths (tests + facade-only smoke).
|
||||||
|
"""
|
||||||
|
import gtsam
|
||||||
|
|
||||||
|
return gtsam.Pose3(np.asarray(matrix, dtype=np.float64))
|
||||||
|
|
||||||
|
|
||||||
|
def _grayscale(image: npt.NDArray[Any]) -> npt.NDArray[Any]:
|
||||||
|
"""Coerce a NavCameraFrame image to OpenCV's expected 2D ``uint8``.
|
||||||
|
|
||||||
|
NavCameraFrame.image is permitted to be 2-D (already grayscale)
|
||||||
|
or 3-D (HxWx{1,3,4}); OpenCV's KLT path expects 2-D uint8. Color
|
||||||
|
images are routed through ``cv2.cvtColor`` so this strategy works
|
||||||
|
against both monochrome industrial cameras AND the standard
|
||||||
|
BGR-coded test fixtures.
|
||||||
|
"""
|
||||||
|
arr = np.ascontiguousarray(image)
|
||||||
|
if arr.dtype != np.uint8:
|
||||||
|
# Convert any non-uint8 type via clipping + cast. OpenCV's KLT
|
||||||
|
# internals only accept uint8; silently routing floats through
|
||||||
|
# would hide a calibration bug.
|
||||||
|
if np.issubdtype(arr.dtype, np.floating):
|
||||||
|
arr = np.clip(arr * 255.0, 0, 255).astype(np.uint8)
|
||||||
|
else:
|
||||||
|
arr = arr.astype(np.uint8)
|
||||||
|
if arr.ndim == 2:
|
||||||
|
return arr
|
||||||
|
if arr.ndim == 3:
|
||||||
|
channels = arr.shape[2]
|
||||||
|
if channels == 1:
|
||||||
|
return arr.reshape(arr.shape[0], arr.shape[1])
|
||||||
|
if channels in (3, 4):
|
||||||
|
code = cv2.COLOR_BGR2GRAY if channels == 3 else cv2.COLOR_BGRA2GRAY
|
||||||
|
return cv2.cvtColor(arr, code)
|
||||||
|
raise VioFatalError(
|
||||||
|
f"KltRansacStrategy: NavCameraFrame.image has unsupported shape "
|
||||||
|
f"{arr.shape}; expected 2-D or 3-D with 1/3/4 channels."
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _intrinsics_3x3(calibration: CameraCalibration) -> np.ndarray:
|
||||||
|
"""Pull the 3x3 intrinsics matrix from a CameraCalibration DTO.
|
||||||
|
|
||||||
|
The DTO stores ``intrinsics_3x3`` as ``Any`` so any list / tuple /
|
||||||
|
ndarray that coerces to (3, 3) is accepted. Anything else fails
|
||||||
|
BEFORE OpenCV would surface a less-actionable ``cv2.error``.
|
||||||
|
"""
|
||||||
|
K = np.asarray(calibration.intrinsics_3x3, dtype=np.float64)
|
||||||
|
if K.shape != (3, 3):
|
||||||
|
raise VioFatalError(
|
||||||
|
f"KltRansacStrategy: CameraCalibration.intrinsics_3x3 must be "
|
||||||
|
f"3x3; got shape {K.shape}"
|
||||||
|
)
|
||||||
|
return K
|
||||||
|
|
||||||
|
|
||||||
|
class KltRansacStrategy:
|
||||||
|
"""Mandatory simple-baseline :class:`VioStrategy` for E-C1 (AZ-334).
|
||||||
|
|
||||||
|
Constructor matches the AZ-331 composition-root factory shape::
|
||||||
|
|
||||||
|
KltRansacStrategy(config: Config, *, fdr_client: FdrClient)
|
||||||
|
|
||||||
|
Other dependencies (KLT/RANSAC sub-config, logger, IMU preintegrator)
|
||||||
|
are resolved internally from ``config`` and the per-call
|
||||||
|
:class:`CameraCalibration`. The preintegrator is lazily constructed
|
||||||
|
on the first :meth:`process_frame` call (it requires the calibration
|
||||||
|
to read the IMU noise model).
|
||||||
|
|
||||||
|
Concurrency: single-threaded by Protocol invariant. One instance
|
||||||
|
per camera-ingest writer thread; concurrent ``process_frame`` calls
|
||||||
|
are undefined behaviour.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
config: Config,
|
||||||
|
*,
|
||||||
|
fdr_client: FdrClient,
|
||||||
|
clock: Clock | None = None,
|
||||||
|
) -> None:
|
||||||
|
c1_block = config.components["c1_vio"]
|
||||||
|
if c1_block.strategy != _STRATEGY_LABEL:
|
||||||
|
raise VioFatalError(
|
||||||
|
f"KltRansacStrategy constructed with config.strategy="
|
||||||
|
f"{c1_block.strategy!r}; expected {_STRATEGY_LABEL!r}. "
|
||||||
|
"The AZ-331 factory is the only sanctioned constructor."
|
||||||
|
)
|
||||||
|
|
||||||
|
self._config = config
|
||||||
|
self._fdr = fdr_client
|
||||||
|
self._clock: Clock = clock if clock is not None else WallClock()
|
||||||
|
self._logger = get_logger(_LOGGER_COMPONENT)
|
||||||
|
self._lost_frame_threshold: int = c1_block.lost_frame_threshold
|
||||||
|
self._warm_start_max_frames: int = c1_block.warm_start_max_frames
|
||||||
|
self._cfg: KltRansacConfig = c1_block.klt_ransac
|
||||||
|
|
||||||
|
# Per-frame state.
|
||||||
|
self._prev_gray: np.ndarray | None = None
|
||||||
|
self._prev_features: np.ndarray | None = None
|
||||||
|
self._calibration: CameraCalibration | None = None
|
||||||
|
self._preintegrator: ImuPreintegrator | None = None
|
||||||
|
self._frames_since_warmup: int = 0
|
||||||
|
self._consecutive_lost: int = 0
|
||||||
|
self._latest_bias: ImuBias = ImuBias(
|
||||||
|
accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0)
|
||||||
|
)
|
||||||
|
self._reported_state: VioState = VioState.INIT
|
||||||
|
self._last_emitted_state: VioState | None = None
|
||||||
|
# Last frame's covariance Frobenius norm — used to verify the
|
||||||
|
# honest-covariance monotonicity invariant in DEGRADED operation.
|
||||||
|
# NOT a covariance floor (AC-9 forbids one); this is a read-only
|
||||||
|
# diagnostic checked at the end of process_frame.
|
||||||
|
self._last_cov_frobenius: float = 0.0
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Public Protocol surface.
|
||||||
|
|
||||||
|
def process_frame(
|
||||||
|
self,
|
||||||
|
frame: NavCameraFrame,
|
||||||
|
imu: ImuWindow,
|
||||||
|
calibration: CameraCalibration,
|
||||||
|
) -> VioOutput:
|
||||||
|
"""Hot-path call — one per nav-camera frame.
|
||||||
|
|
||||||
|
Steps (see task spec § Outcome for the canonical narrative):
|
||||||
|
|
||||||
|
1. Push every IMU sample in the window through the AZ-276
|
||||||
|
preintegrator (strict-monotonic guard lives in the helper).
|
||||||
|
2. Convert the frame to grayscale ``uint8``.
|
||||||
|
3. First-frame path: seed features + return identity-pose
|
||||||
|
``VioOutput`` with INIT state.
|
||||||
|
4. Subsequent-frame path: KLT-track the prior features, run
|
||||||
|
the AZ-282 :class:`RansacFilter` inlier rejection, recover
|
||||||
|
the essential-matrix + pose, build the ``VioOutput`` with
|
||||||
|
residual-scatter covariance.
|
||||||
|
"""
|
||||||
|
self._calibration = calibration
|
||||||
|
frame_id_str = str(frame.frame_id)
|
||||||
|
emitted_at_ns = self._clock.monotonic_ns()
|
||||||
|
|
||||||
|
# 1. Push IMU samples — bias propagation only; KLT itself is
|
||||||
|
# vision-only so the latest_bias stays equal to the most recent
|
||||||
|
# warm-start hint until the C5 estimator pushes a fresh value
|
||||||
|
# through `reset_to_warm_start`.
|
||||||
|
self._ensure_preintegrator(calibration)
|
||||||
|
try:
|
||||||
|
assert self._preintegrator is not None # narrow for mypy
|
||||||
|
self._preintegrator.integrate_window(imu)
|
||||||
|
except ImuPreintegrationError as exc:
|
||||||
|
raise VioFatalError(
|
||||||
|
f"KltRansacStrategy: IMU preintegrator rejected window at "
|
||||||
|
f"{frame_id_str!r}: {exc}"
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
# 2. Grayscale.
|
||||||
|
try:
|
||||||
|
curr_gray = _grayscale(frame.image)
|
||||||
|
except cv2.error as exc:
|
||||||
|
raise VioFatalError(
|
||||||
|
f"KltRansacStrategy: OpenCV failed to grayscale frame "
|
||||||
|
f"{frame_id_str!r}: {exc}"
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
# 3. First-frame seed + INIT emit.
|
||||||
|
if self._prev_gray is None or self._prev_features is None:
|
||||||
|
self._seed_features(curr_gray, frame_id_str)
|
||||||
|
self._prev_gray = curr_gray
|
||||||
|
return self._first_frame_output(frame_id_str, emitted_at_ns)
|
||||||
|
|
||||||
|
# 4. KLT track features into current frame.
|
||||||
|
try:
|
||||||
|
tracked = self._track_features(self._prev_gray, curr_gray, self._prev_features)
|
||||||
|
except cv2.error as exc:
|
||||||
|
raise VioFatalError(
|
||||||
|
f"KltRansacStrategy: OpenCV KLT track failed at {frame_id_str!r}: {exc}"
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
prior_feature_count = int(self._prev_features.shape[0])
|
||||||
|
|
||||||
|
# 4.5 Floor check — essential matrix requires >=5 correspondences.
|
||||||
|
if tracked.shape[0] < _ESSENTIAL_MATRIX_DOF:
|
||||||
|
return self._pose_recovery_failed(
|
||||||
|
frame_id_str,
|
||||||
|
emitted_at_ns,
|
||||||
|
prior_feature_count=prior_feature_count,
|
||||||
|
reason="insufficient_tracked_features",
|
||||||
|
)
|
||||||
|
|
||||||
|
# 5. AZ-282 RANSAC pre-filter — separate stage from the
|
||||||
|
# findEssentialMat internal RANSAC.
|
||||||
|
try:
|
||||||
|
ransac_result = RansacFilter.filter_correspondences(
|
||||||
|
tracked,
|
||||||
|
self._cfg.essential_matrix_ransac_threshold_px,
|
||||||
|
int(self._cfg.min_features_for_pose * self._cfg.ransac_inlier_ratio),
|
||||||
|
)
|
||||||
|
except RansacFilterError as exc:
|
||||||
|
# Helper rejected the input (degenerate correspondences,
|
||||||
|
# OpenCV internal failure). Treat as pose-recovery failure.
|
||||||
|
return self._pose_recovery_failed(
|
||||||
|
frame_id_str,
|
||||||
|
emitted_at_ns,
|
||||||
|
prior_feature_count=prior_feature_count,
|
||||||
|
reason=f"ransac_filter_error: {exc}",
|
||||||
|
)
|
||||||
|
|
||||||
|
if ransac_result.inlier_count < _ESSENTIAL_MATRIX_DOF:
|
||||||
|
return self._pose_recovery_failed(
|
||||||
|
frame_id_str,
|
||||||
|
emitted_at_ns,
|
||||||
|
prior_feature_count=prior_feature_count,
|
||||||
|
reason="insufficient_inliers_after_ransac",
|
||||||
|
)
|
||||||
|
|
||||||
|
# 6. Essential-matrix + recoverPose.
|
||||||
|
K = _intrinsics_3x3(calibration)
|
||||||
|
inliers = ransac_result.inlier_correspondences
|
||||||
|
pts_prev = inliers[:, :2].astype(np.float64, copy=False)
|
||||||
|
pts_curr = inliers[:, 2:].astype(np.float64, copy=False)
|
||||||
|
try:
|
||||||
|
E, em_mask = cv2.findEssentialMat(
|
||||||
|
pts_prev,
|
||||||
|
pts_curr,
|
||||||
|
K,
|
||||||
|
method=cv2.RANSAC,
|
||||||
|
threshold=float(self._cfg.essential_matrix_ransac_threshold_px),
|
||||||
|
)
|
||||||
|
if E is None or np.asarray(E).shape != (3, 3):
|
||||||
|
return self._pose_recovery_failed(
|
||||||
|
frame_id_str,
|
||||||
|
emitted_at_ns,
|
||||||
|
prior_feature_count=prior_feature_count,
|
||||||
|
reason="find_essential_mat_no_model",
|
||||||
|
)
|
||||||
|
_retval, R, t, _final_mask = cv2.recoverPose(E, pts_prev, pts_curr, K, mask=em_mask)
|
||||||
|
except cv2.error as exc:
|
||||||
|
# AC-4: rewrap raw cv2.error as VioFatalError.
|
||||||
|
raise VioFatalError(
|
||||||
|
f"KltRansacStrategy: OpenCV essential-matrix / recoverPose "
|
||||||
|
f"failed at {frame_id_str!r}: {exc}"
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
if R is None or t is None:
|
||||||
|
return self._pose_recovery_failed(
|
||||||
|
frame_id_str,
|
||||||
|
emitted_at_ns,
|
||||||
|
prior_feature_count=prior_feature_count,
|
||||||
|
reason="recover_pose_no_solution",
|
||||||
|
)
|
||||||
|
|
||||||
|
# 7. Build SE(3) from (R, t).
|
||||||
|
pose_4x4 = np.eye(4, dtype=np.float64)
|
||||||
|
pose_4x4[:3, :3] = np.asarray(R, dtype=np.float64)
|
||||||
|
pose_4x4[:3, 3] = np.asarray(t, dtype=np.float64).flatten()
|
||||||
|
pose = _se3_from_4x4(pose_4x4)
|
||||||
|
|
||||||
|
# 8. Final inlier count for state classification + covariance.
|
||||||
|
final_inlier_count = (
|
||||||
|
int(np.count_nonzero(em_mask)) if em_mask is not None else ransac_result.inlier_count
|
||||||
|
)
|
||||||
|
if final_inlier_count < _ESSENTIAL_MATRIX_DOF:
|
||||||
|
final_inlier_count = ransac_result.inlier_count
|
||||||
|
|
||||||
|
# 9. Estimate covariance from the AZ-282 median residual +
|
||||||
|
# inlier-count penalty. Honest-covariance invariant (AC-9): no
|
||||||
|
# client-side floor; the formula is residual_var / DOF + small
|
||||||
|
# inlier-count term so the cov grows monotonically as inliers
|
||||||
|
# drop or residuals scatter.
|
||||||
|
cov = self._estimate_covariance(
|
||||||
|
median_residual_px=ransac_result.median_residual_px,
|
||||||
|
inlier_count=final_inlier_count,
|
||||||
|
)
|
||||||
|
|
||||||
|
# 10. Build VioOutput.
|
||||||
|
fq = FeatureQuality(
|
||||||
|
tracked=int(final_inlier_count),
|
||||||
|
new=int(max(0, prior_feature_count - tracked.shape[0])),
|
||||||
|
lost=int(max(0, prior_feature_count - final_inlier_count)),
|
||||||
|
mean_parallax=_safe_float(ransac_result.median_residual_px),
|
||||||
|
mre_px=_safe_float(ransac_result.median_residual_px),
|
||||||
|
)
|
||||||
|
|
||||||
|
self._latest_bias = self._latest_bias # unchanged — KLT is vision-only
|
||||||
|
vio_output = VioOutput(
|
||||||
|
frame_id=frame_id_str,
|
||||||
|
relative_pose_T=pose,
|
||||||
|
pose_covariance_6x6=cov,
|
||||||
|
imu_bias=self._latest_bias,
|
||||||
|
feature_quality=fq,
|
||||||
|
emitted_at_ns=emitted_at_ns,
|
||||||
|
)
|
||||||
|
|
||||||
|
# 11. Success path — reset lost counter, classify state.
|
||||||
|
self._consecutive_lost = 0
|
||||||
|
new_state = self._classify_state(fq)
|
||||||
|
if new_state != self._reported_state:
|
||||||
|
self._reported_state = new_state
|
||||||
|
self._emit_transition(new_state, frame_id_str)
|
||||||
|
|
||||||
|
if new_state in (VioState.INIT, VioState.TRACKING):
|
||||||
|
self._frames_since_warmup += 1
|
||||||
|
|
||||||
|
# 12. Re-seed features for next frame — KLT/RANSAC re-detects
|
||||||
|
# corners every frame to keep the feature count bounded and to
|
||||||
|
# mitigate the well-known KLT drift-away problem on long tracks.
|
||||||
|
try:
|
||||||
|
self._seed_features(curr_gray, frame_id_str)
|
||||||
|
except cv2.error as exc:
|
||||||
|
raise VioFatalError(
|
||||||
|
f"KltRansacStrategy: OpenCV goodFeaturesToTrack failed "
|
||||||
|
f"at {frame_id_str!r}: {exc}"
|
||||||
|
) from exc
|
||||||
|
self._prev_gray = curr_gray
|
||||||
|
self._last_cov_frobenius = float(np.linalg.norm(cov, ord="fro"))
|
||||||
|
|
||||||
|
if self._cfg.per_frame_debug_log:
|
||||||
|
self._logger.debug(
|
||||||
|
"klt_ransac.process_frame",
|
||||||
|
extra={
|
||||||
|
"component": _LOGGER_COMPONENT,
|
||||||
|
"kind": "vio.tick",
|
||||||
|
"frame_id": frame_id_str,
|
||||||
|
"kv": {
|
||||||
|
"state": self._reported_state.value,
|
||||||
|
"tracked": fq.tracked,
|
||||||
|
"mre_px": fq.mre_px,
|
||||||
|
"cov_frobenius": self._last_cov_frobenius,
|
||||||
|
"emitted_at_ns": vio_output.emitted_at_ns,
|
||||||
|
},
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
return vio_output
|
||||||
|
|
||||||
|
def reset_to_warm_start(self, hint: WarmStartPose) -> None:
|
||||||
|
"""Destructive re-init from an F8-reboot warm-start hint.
|
||||||
|
|
||||||
|
Clears the prior-frame KLT buffer + re-seeds the IMU bias via
|
||||||
|
the AZ-276 preintegrator. Idempotent across consecutive calls
|
||||||
|
(AC-4) — a second call without an intervening
|
||||||
|
:meth:`process_frame` reseats state without raising.
|
||||||
|
"""
|
||||||
|
try:
|
||||||
|
_ = np.asarray(hint.body_T_world.matrix(), dtype=np.float64)
|
||||||
|
except AttributeError as exc:
|
||||||
|
raise VioFatalError(
|
||||||
|
"KltRansacStrategy.reset_to_warm_start: hint.body_T_world is "
|
||||||
|
"not a gtsam.Pose3 (missing .matrix())"
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
# Seed the bias on the preintegrator IF it has been constructed;
|
||||||
|
# if `process_frame` has never been called yet, the
|
||||||
|
# preintegrator does not exist (it needs the per-call
|
||||||
|
# calibration). The hint bias is still recorded so the FIRST
|
||||||
|
# `process_frame` (which builds the preintegrator) starts with
|
||||||
|
# the right value via `reset_with_bias` after construction.
|
||||||
|
if self._preintegrator is not None:
|
||||||
|
try:
|
||||||
|
self._preintegrator.reset_with_bias(hint.bias)
|
||||||
|
except ImuPreintegrationError as exc:
|
||||||
|
raise VioFatalError(
|
||||||
|
f"KltRansacStrategy: preintegrator rejected warm-start "
|
||||||
|
f"bias reset: {exc}"
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
self._latest_bias = hint.bias
|
||||||
|
self._prev_gray = None
|
||||||
|
self._prev_features = None
|
||||||
|
self._frames_since_warmup = 0
|
||||||
|
self._consecutive_lost = 0
|
||||||
|
self._reported_state = VioState.INIT
|
||||||
|
self._last_cov_frobenius = 0.0
|
||||||
|
self._emit_transition(VioState.INIT, frame_id="")
|
||||||
|
|
||||||
|
def health_snapshot(self) -> VioHealth:
|
||||||
|
"""Most-recent health state — no OpenCV call (cheap)."""
|
||||||
|
return VioHealth(
|
||||||
|
state=self._reported_state,
|
||||||
|
consecutive_lost=self._consecutive_lost,
|
||||||
|
bias_norm=_bias_norm(self._latest_bias),
|
||||||
|
)
|
||||||
|
|
||||||
|
def current_strategy_label(self) -> Literal["okvis2", "vins_mono", "klt_ransac"]:
|
||||||
|
return _STRATEGY_LABEL
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Internal helpers.
|
||||||
|
|
||||||
|
def _ensure_preintegrator(self, calibration: CameraCalibration) -> None:
|
||||||
|
"""Build the AZ-276 preintegrator on the first frame.
|
||||||
|
|
||||||
|
The preintegrator needs the per-deployment IMU noise model from
|
||||||
|
``calibration.metadata``; that's only available once the camera-
|
||||||
|
ingest loop has the first ``NavCameraFrame``. Subsequent frames
|
||||||
|
reuse the same instance.
|
||||||
|
"""
|
||||||
|
if self._preintegrator is None:
|
||||||
|
self._preintegrator = make_imu_preintegrator(calibration)
|
||||||
|
# Seed bias if a warm-start hint was applied before the
|
||||||
|
# first frame (the hint cannot reach the preintegrator
|
||||||
|
# earlier because the preintegrator does not exist yet).
|
||||||
|
if (
|
||||||
|
self._latest_bias.accel_bias != (0.0, 0.0, 0.0)
|
||||||
|
or self._latest_bias.gyro_bias != (0.0, 0.0, 0.0)
|
||||||
|
):
|
||||||
|
self._preintegrator.reset_with_bias(self._latest_bias)
|
||||||
|
|
||||||
|
def _seed_features(self, gray: np.ndarray, frame_id_str: str) -> None:
|
||||||
|
"""Detect fresh corners + store as the prior-frame feature buffer."""
|
||||||
|
features = cv2.goodFeaturesToTrack(
|
||||||
|
gray,
|
||||||
|
maxCorners=int(self._cfg.max_corners),
|
||||||
|
qualityLevel=0.01,
|
||||||
|
minDistance=7,
|
||||||
|
)
|
||||||
|
if features is None:
|
||||||
|
# Empty detection — record an empty buffer so the next
|
||||||
|
# `process_frame` enters the "insufficient_tracked_features"
|
||||||
|
# branch and ticks the lost counter.
|
||||||
|
self._prev_features = np.empty((0, 1, 2), dtype=np.float32)
|
||||||
|
else:
|
||||||
|
self._prev_features = np.asarray(features, dtype=np.float32)
|
||||||
|
|
||||||
|
def _track_features(
|
||||||
|
self,
|
||||||
|
prev_gray: np.ndarray,
|
||||||
|
curr_gray: np.ndarray,
|
||||||
|
prev_features: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Run pyramidal Lucas-Kanade + return surviving (N, 4) correspondences."""
|
||||||
|
if prev_features.shape[0] == 0:
|
||||||
|
return np.empty((0, 4), dtype=np.float64)
|
||||||
|
|
||||||
|
win = int(self._cfg.klt_window_size_px)
|
||||||
|
new_features, status, _err = cv2.calcOpticalFlowPyrLK(
|
||||||
|
prev_gray,
|
||||||
|
curr_gray,
|
||||||
|
prev_features,
|
||||||
|
None,
|
||||||
|
winSize=(win, win),
|
||||||
|
maxLevel=int(self._cfg.klt_pyramid_levels - 1),
|
||||||
|
)
|
||||||
|
if new_features is None or status is None:
|
||||||
|
return np.empty((0, 4), dtype=np.float64)
|
||||||
|
|
||||||
|
status_flat = status.flatten().astype(bool)
|
||||||
|
if not np.any(status_flat):
|
||||||
|
return np.empty((0, 4), dtype=np.float64)
|
||||||
|
|
||||||
|
prev_pts = prev_features.reshape(-1, 2)[status_flat]
|
||||||
|
curr_pts = np.asarray(new_features).reshape(-1, 2)[status_flat]
|
||||||
|
correspondences = np.hstack(
|
||||||
|
[prev_pts.astype(np.float64, copy=False), curr_pts.astype(np.float64, copy=False)]
|
||||||
|
)
|
||||||
|
return correspondences
|
||||||
|
|
||||||
|
def _first_frame_output(self, frame_id_str: str, emitted_at_ns: int) -> VioOutput:
|
||||||
|
"""Return identity-pose + INIT-state ``VioOutput`` for AC-2.
|
||||||
|
|
||||||
|
Identity SE(3) is the canonical "no motion yet" pose; the
|
||||||
|
covariance is intentionally large so C5 fusion treats this
|
||||||
|
as un-informed (the warm-start hint, not this VioOutput, is
|
||||||
|
what seeds C5's initial estimate).
|
||||||
|
"""
|
||||||
|
identity_pose = _se3_from_4x4(np.eye(4, dtype=np.float64))
|
||||||
|
cov = np.eye(6, dtype=np.float64) * _INIT_STATE_COVARIANCE_SCALAR
|
||||||
|
fq = FeatureQuality(
|
||||||
|
tracked=int(self._prev_features.shape[0]) if self._prev_features is not None else 0,
|
||||||
|
new=int(self._prev_features.shape[0]) if self._prev_features is not None else 0,
|
||||||
|
lost=0,
|
||||||
|
mean_parallax=0.0,
|
||||||
|
mre_px=0.0,
|
||||||
|
)
|
||||||
|
self._frames_since_warmup += 1
|
||||||
|
# Emit the INIT transition exactly once.
|
||||||
|
self._emit_transition(VioState.INIT, frame_id_str)
|
||||||
|
self._last_cov_frobenius = float(np.linalg.norm(cov, ord="fro"))
|
||||||
|
return VioOutput(
|
||||||
|
frame_id=frame_id_str,
|
||||||
|
relative_pose_T=identity_pose,
|
||||||
|
pose_covariance_6x6=cov,
|
||||||
|
imu_bias=self._latest_bias,
|
||||||
|
feature_quality=fq,
|
||||||
|
emitted_at_ns=emitted_at_ns,
|
||||||
|
)
|
||||||
|
|
||||||
|
def _pose_recovery_failed(
|
||||||
|
self,
|
||||||
|
frame_id_str: str,
|
||||||
|
emitted_at_ns: int,
|
||||||
|
*,
|
||||||
|
prior_feature_count: int,
|
||||||
|
reason: str,
|
||||||
|
) -> VioOutput:
|
||||||
|
"""Pose-recovery failure path.
|
||||||
|
|
||||||
|
Ticks the lost counter and either raises (per AC-7) or returns
|
||||||
|
a DEGRADED ``VioOutput`` with inflated covariance. The choice
|
||||||
|
between raise vs. return mirrors :class:`Okvis2Strategy` /
|
||||||
|
:class:`VinsMonoStrategy`: after the lost-frame threshold is
|
||||||
|
exceeded, raise :class:`VioFatalError`; otherwise raise
|
||||||
|
:class:`VioInitializingError`.
|
||||||
|
|
||||||
|
AC-6's "VioOutput IS emitted" path is the OTHER branch (low
|
||||||
|
inliers but pose DID recover) — that path never reaches this
|
||||||
|
helper. This helper is reserved for the AC-7 failed-pose
|
||||||
|
regime.
|
||||||
|
"""
|
||||||
|
self._tick_lost(frame_id_str)
|
||||||
|
if self._reported_state == VioState.LOST:
|
||||||
|
self._emit_transition(VioState.LOST, frame_id_str)
|
||||||
|
raise VioFatalError(
|
||||||
|
f"KltRansacStrategy: exhausted lost-frame budget "
|
||||||
|
f"({self._lost_frame_threshold} consecutive failures) at "
|
||||||
|
f"{frame_id_str!r} ({reason})"
|
||||||
|
)
|
||||||
|
self._emit_transition(self._reported_state, frame_id_str)
|
||||||
|
raise VioInitializingError(
|
||||||
|
f"KltRansacStrategy: pose recovery failed at {frame_id_str!r} "
|
||||||
|
f"({reason}); prior feature count = {prior_feature_count}"
|
||||||
|
)
|
||||||
|
|
||||||
|
def _estimate_covariance(
|
||||||
|
self,
|
||||||
|
*,
|
||||||
|
median_residual_px: float,
|
||||||
|
inlier_count: int,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Honest covariance estimator — see AZ-334 § Outcome step 7.
|
||||||
|
|
||||||
|
Standard textbook approach: ``sigma^2 / DOF`` gives the
|
||||||
|
per-parameter variance, where ``DOF = N_inliers - 5`` (essential
|
||||||
|
matrix has 5 DOF). The ``inlier_count`` penalty term ensures
|
||||||
|
cov Frobenius is strictly monotonic non-decreasing as inliers
|
||||||
|
drop — required by AC-6 + AC-9.
|
||||||
|
|
||||||
|
Returned as a 6x6 diagonal matrix. SPD by construction. The
|
||||||
|
diagonal form is a simplification — it ignores the directional
|
||||||
|
sensitivity of pose error to image residuals (Risk-1). The
|
||||||
|
Step 9 IT-12 comparative report cross-validates against
|
||||||
|
OKVIS2's full block covariance.
|
||||||
|
|
||||||
|
Honest-covariance invariant (AC-9): NO client-side floor or
|
||||||
|
smoother. The formula is deterministic in its inputs.
|
||||||
|
"""
|
||||||
|
# NaN-safe residual sigma — RansacFilter returns NaN for empty
|
||||||
|
# inlier sets; clip to a small positive value so the math stays
|
||||||
|
# well-defined (the caller path guarantees inlier_count >= 5
|
||||||
|
# before reaching this helper, so the NaN branch is defensive).
|
||||||
|
sigma_sq = (
|
||||||
|
float(median_residual_px) ** 2
|
||||||
|
if median_residual_px == median_residual_px and median_residual_px > 0.0
|
||||||
|
else 1e-6
|
||||||
|
)
|
||||||
|
dof = max(inlier_count - _ESSENTIAL_MATRIX_DOF, 1)
|
||||||
|
# Inlier-count penalty: monotonically increasing as inlier_count
|
||||||
|
# drops. The coefficient is tied to the RANSAC threshold so the
|
||||||
|
# penalty is in the same pixel-residual units as sigma_sq.
|
||||||
|
inlier_penalty = (
|
||||||
|
float(self._cfg.essential_matrix_ransac_threshold_px)
|
||||||
|
/ max(int(inlier_count), 1)
|
||||||
|
)
|
||||||
|
scalar = (sigma_sq + inlier_penalty) / dof
|
||||||
|
return np.eye(6, dtype=np.float64) * scalar
|
||||||
|
|
||||||
|
def _classify_state(self, fq: FeatureQuality) -> VioState:
|
||||||
|
"""Map per-frame feature quality + warmup to a :class:`VioState`."""
|
||||||
|
if self._reported_state == VioState.INIT and (
|
||||||
|
self._frames_since_warmup + 1 < self._warm_start_max_frames
|
||||||
|
):
|
||||||
|
return VioState.INIT
|
||||||
|
if fq.tracked < self._cfg.min_features_for_pose:
|
||||||
|
return VioState.DEGRADED
|
||||||
|
return VioState.TRACKING
|
||||||
|
|
||||||
|
def _tick_lost(self, frame_id_str: str) -> None:
|
||||||
|
"""Tick the lost-frame counter + transition state if needed.
|
||||||
|
|
||||||
|
Mirrors :class:`Okvis2Strategy._tick_lost` exactly; the
|
||||||
|
post-AZ-334 hygiene PBI (Batch 53 review F1) will consolidate.
|
||||||
|
"""
|
||||||
|
self._consecutive_lost += 1
|
||||||
|
if self._consecutive_lost >= self._lost_frame_threshold:
|
||||||
|
self._reported_state = VioState.LOST
|
||||||
|
elif self._reported_state == VioState.TRACKING:
|
||||||
|
self._reported_state = VioState.DEGRADED
|
||||||
|
|
||||||
|
def _emit_transition(self, new_state: VioState, frame_id: str) -> None:
|
||||||
|
"""Emit an FDR ``vio.health`` record IFF the state changed (AC-10).
|
||||||
|
|
||||||
|
Steady-state frames produce no record — only transitions
|
||||||
|
through (INIT, TRACKING, DEGRADED, LOST) are FDR-stamped.
|
||||||
|
"""
|
||||||
|
if self._last_emitted_state == new_state:
|
||||||
|
return
|
||||||
|
self._last_emitted_state = new_state
|
||||||
|
record = FdrRecord(
|
||||||
|
schema_version=CURRENT_SCHEMA_VERSION,
|
||||||
|
ts=_now_iso(),
|
||||||
|
producer_id=_PRODUCER_ID,
|
||||||
|
kind="vio.health",
|
||||||
|
payload={
|
||||||
|
"state": new_state.value,
|
||||||
|
"consecutive_lost": self._consecutive_lost,
|
||||||
|
"bias_norm": _bias_norm(self._latest_bias),
|
||||||
|
"strategy_label": _STRATEGY_LABEL,
|
||||||
|
"frame_id": frame_id,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
self._fdr.enqueue(record)
|
||||||
|
|
||||||
|
|
||||||
|
def _safe_float(value: float) -> float:
|
||||||
|
"""NaN-safe float coercion — RansacFilter returns NaN for empty inliers."""
|
||||||
|
if value != value: # NaN check without numpy
|
||||||
|
return 0.0
|
||||||
|
return float(value)
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -250,13 +250,21 @@ def test_ac5_build_vio_strategy_flag_off_no_import(
|
|||||||
|
|
||||||
|
|
||||||
# Which strategies still have NO concrete Python module on disk?
|
# Which strategies still have NO concrete Python module on disk?
|
||||||
# Once an AZ-332 / AZ-333 / AZ-334 implementation lands, the
|
# All three strategies (AZ-332 / AZ-333 / AZ-334) have landed; tuple
|
||||||
# `flag_on_but_module_missing` semantic shifts: the factory's import
|
# remains as a tombstone for git-blame archaeology of the build-time
|
||||||
# succeeds, the constructor fails on missing native binding or other
|
# gating evolution. Once removed, the parametrisation below collapses
|
||||||
# prerequisite. We assert the meaningful-error-before-first-frame
|
# to two branches (native-binding vs pure-Python).
|
||||||
# property holds for BOTH cases — the exception class differs by
|
_STRATEGIES_WITHOUT_PY_MODULE: tuple[str, ...] = ()
|
||||||
# strategy.
|
|
||||||
_STRATEGIES_WITHOUT_PY_MODULE: tuple[str, ...] = ("klt_ransac",)
|
# Strategies whose concrete implementation has NO native binding —
|
||||||
|
# the AZ-334 KLT/RANSAC simple-baseline is pure-Python over OpenCV.
|
||||||
|
# When ``BUILD_KLT_RANSAC=ON`` and the module is on disk, the
|
||||||
|
# constructor succeeds end-to-end (no native ``.so`` to be missing).
|
||||||
|
# The AC-5 spirit (meaningful error before first frame) is still
|
||||||
|
# satisfied: the only way construction fails for klt_ransac is the
|
||||||
|
# ``BUILD_*=OFF`` path which is already covered by
|
||||||
|
# :func:`test_ac5_build_vio_strategy_flag_off_no_import`.
|
||||||
|
_STRATEGIES_WITHOUT_NATIVE_BINDING: tuple[str, ...] = ("klt_ransac",)
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("strategy", sorted(_STRATEGY_MODULES))
|
@pytest.mark.parametrize("strategy", sorted(_STRATEGY_MODULES))
|
||||||
@@ -272,11 +280,22 @@ def test_ac5_build_vio_strategy_flag_on_but_module_missing(
|
|||||||
with pytest.raises(StrategyNotAvailableError) as exc_info:
|
with pytest.raises(StrategyNotAvailableError) as exc_info:
|
||||||
build_vio_strategy(config, fdr_client=object())
|
build_vio_strategy(config, fdr_client=object())
|
||||||
assert strategy in str(exc_info.value)
|
assert strategy in str(exc_info.value)
|
||||||
|
elif strategy in _STRATEGIES_WITHOUT_NATIVE_BINDING:
|
||||||
|
# Module IS implemented AND has no native binding (AZ-334
|
||||||
|
# KLT/RANSAC). Constructor succeeds without raising; the
|
||||||
|
# only failure mode the AC-5 test guards against does not
|
||||||
|
# apply to a pure-Python strategy. We assert the construction
|
||||||
|
# produces a real :class:`VioStrategy` instance to keep the
|
||||||
|
# test branch non-trivial.
|
||||||
|
instance = build_vio_strategy(config, fdr_client=object())
|
||||||
|
assert isinstance(instance, VioStrategy)
|
||||||
|
assert instance.current_strategy_label() == strategy
|
||||||
else:
|
else:
|
||||||
# Module IS implemented (AZ-332). Factory import succeeds, then
|
# Module IS implemented (AZ-332 / AZ-333). Factory import
|
||||||
# the strategy constructor fails on missing native binding —
|
# succeeds, then the strategy constructor fails on missing
|
||||||
# which the strategy MUST surface as VioFatalError BEFORE any
|
# native binding — which the strategy MUST surface as
|
||||||
# frame is processed (the AC-5 spirit: no silent fall-through).
|
# VioFatalError BEFORE any frame is processed (the AC-5
|
||||||
|
# spirit: no silent fall-through).
|
||||||
with pytest.raises(VioFatalError) as exc_info:
|
with pytest.raises(VioFatalError) as exc_info:
|
||||||
build_vio_strategy(config, fdr_client=object())
|
build_vio_strategy(config, fdr_client=object())
|
||||||
assert "native binding" in str(exc_info.value)
|
assert "native binding" in str(exc_info.value)
|
||||||
|
|||||||
Reference in New Issue
Block a user