[AZ-332] C1 OKVIS2 Strategy: facade + binding skeleton

Python facade (`Okvis2Strategy`) is production-quality and satisfies
AZ-331's `VioStrategy` protocol; full AC-1..10 coverage with
AC-9 + NFR-perf marked `tier2`. The C++ pybind11 binding compiles
and loads but throws `OkvisFatalException("estimator not yet wired")`
on first `add_frame` — the `okvis::ThreadedKFVio` wiring is a tier2
follow-up the Step-15 Product Completeness Gate is expected to track
as a remediation task.

Resolved contradictions:

* Constructor signature aligned with the AZ-331 factory: `(config, *,
  fdr_client, clock=None)`. Calibration / preintegrator / logger
  built internally from config. No churn on AZ-331.
* IMU substrate: OKVIS2 owns its internal estimator IMU integration;
  the AZ-276 `ImuPreintegrator` is a separate substrate consumed by
  E-C5's fusion graph. Single source of truth lives at the sample
  stream, not the integrator instance.
* FDR API: `FdrClient.enqueue(record)` with new `vio.health` kind
  added to AZ-272 `KNOWN_PAYLOAD_KEYS`.

CI matrix forces `-DBUILD_OKVIS2=OFF` until the tier2 wiring task
brings Ceres / SuiteSparse / OKVIS2 vendored submodules into the
Linux build.

Files: 17 added/modified across `c1_vio/`, `fdr_client/records.py`,
`cpp/okvis2/CMakeLists.txt`, CI workflow, AZ-332 task spec
(implementation-notes section), batch 23 report.

Tests: 17 new (15 tier1 + 2 tier2). Full Tier-1 suite: 1109 pass,
2 skipped (env), 2 deselected (tier2). No regressions.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-12 09:56:45 +03:00
parent 9c35776bcb
commit 1ebab29a4f
19 changed files with 2083 additions and 49 deletions
@@ -32,18 +32,18 @@ This task delivers the canonical production VIO. The other two strategies (VINS-
- An `Okvis2Strategy` class at `src/gps_denied_onboard/components/c1_vio/okvis2.py` conforming to the `VioStrategy` Protocol from AZ-331; `current_strategy_label() == "okvis2"`.
- A pybind11 wrapper at `src/gps_denied_onboard/components/c1_vio/_native/okvis2_binding.cpp` exposing the OKVIS2 C++ estimator (`okvis::ThreadedKFVio` or equivalent in the pinned upstream HEAD) to Python. The wrapper is built by CMake under `cpp/okvis2/` (build-time gated by `BUILD_OKVIS2`); the resulting `.so` is imported lazily inside `okvis2.py`.
- Constructor `__init__(self, *, calibration: CameraCalibration, preintegrator: ImuPreintegrator, fdr_client: FdrClient, logger: Logger, config: Okvis2Config)` — all dependencies constructor-injected per ADR-009. `Okvis2Config` (`@dataclass(frozen=True)`) carries the OKVIS2-specific knobs (sliding-window size K ∈ [10, 20], keyframe-decision parallax threshold, RANSAC inlier ratio, max optimisation iterations) loaded from `config.vio.okvis2.*` via AZ-269.
- Constructor `__init__(self, config: Config, *, fdr_client: FdrClient, clock: Clock | None = None)` — matches the AZ-331 composition-root factory shape (resolved 2026-05-12 against the existing factory call site `strategy_cls(config, fdr_client=fdr_client)`). Other dependencies (logger, camera calibration, IMU preintegrator substrate, OKVIS2-specific sub-config) are resolved internally from `config`. `Okvis2Config` (`@dataclass(frozen=True)`) carries the OKVIS2-specific knobs (sliding-window size K ∈ [10, 20], keyframe-decision parallax threshold, RANSAC inlier ratio, max optimisation iterations, degraded-feature threshold, per-frame debug log) loaded from `config.components.c1_vio.okvis2.*` via AZ-269 / AZ-331. `clock` defaults to `WallClock()` for live + REALTIME-replay tiers; replay-ASAP composition injects a `TlogDerivedClock` (Invariant 2 of the replay contract).
- `process_frame(frame, imu, calibration) -> VioOutput`:
1. Append IMU samples to the injected `ImuPreintegrator` (strict-monotonic guarded; `ImuPreintegrationError` rewraps to `VioFatalError`).
1. Push every IMU sample in the window into the OKVIS2 backend via `add_imu` (strict-monotonic enforced on the C++ side). OKVIS2 owns its own internal IMU integration for the VIO estimator's per-keyframe factor — the AZ-276 `ImuPreintegrator` is a *separate* substrate used by E-C5's fusion graph, NOT the input to OKVIS2's internal estimator. The "single source of IMU truth" invariant operates at the *sample-stream* level (one IMU producer), not at the integrator-instance level.
2. Feed the nav-camera frame to OKVIS2 via the pybind11 `add_frame` wrapper.
3. If OKVIS2 emits a new estimator update, extract the relative pose (SE(3) via `helpers.se3_utils`), the 6×6 covariance from OKVIS2's internal Hessian (or marginalised block per upstream API), the latest IMU bias, and the feature-quality summary (tracked / new / lost / mean parallax / per-frame MRE).
4. Build and return `VioOutput` with `frame_id` echoed.
4. Build and return `VioOutput` with `frame_id` echoed (stringified).
5. Emit per-frame DEBUG log (off by default) with backbone identity + elapsed milliseconds; emit WARN log when degraded covariance is detected (per `health_snapshot` heuristic); emit ERROR log on `VioFatalError`.
- `reset_to_warm_start(hint)`: tears down the current OKVIS2 estimator instance (releases C++ resources), constructs a fresh estimator, seeds the IMU bias from `hint.bias`, seeds the initial body-to-world pose from `hint.body_T_world`, and seeds the velocity from `hint.velocity_b`. The next `config.vio.warm_start_max_frames` frames are allowed to converge before the strategy reports `state == TRACKING` (AC-5.1). Calling `reset_to_warm_start` is idempotent across consecutive calls (the second call re-resets cleanly).
- `health_snapshot()` returns `VioHealth(state, consecutive_lost, bias_norm)` derived from OKVIS2's internal tracker state: `INIT` until enough keyframes are accumulated, `TRACKING` while the optimisation converges, `DEGRADED` when feature count drops below `config.vio.okvis2.degraded_feature_threshold` or covariance Frobenius norm exceeds 2× steady-state, `LOST` after `config.vio.lost_frame_threshold` consecutive frames without a successful update.
- The honest-covariance invariant (Protocol Invariant) is enforced behaviourally: the strategy MUST NOT shrink the reported covariance during a `DEGRADED` window (the OKVIS2 estimator's covariance is read directly; no smoothing or floor is applied that would mask degradation).
- Error envelope is closed: every OKVIS2 / pybind11 / Eigen exception is caught inside `process_frame` / `reset_to_warm_start` and rewrapped into the `VioError` family (`VioInitializingError` while INIT, `VioFatalError` on backend-init failure or sustained LOST).
- All FDR records emitted via the injected `FdrClient` use the `kind="vio.health"` schema from AZ-272; per-frame DEBUG goes to stdout/journald only (per description.md § 9 logging strategy).
- All FDR records emitted via the injected `FdrClient.enqueue(record)` use the new `kind="vio.health"` schema (added to AZ-272's `KNOWN_PAYLOAD_KEYS` by this task — payload: `state`, `consecutive_lost`, `bias_norm`, `strategy_label`, `frame_id`); per-frame DEBUG goes to stdout/journald only (per description.md § 9 logging strategy).
## Scope
@@ -74,6 +74,19 @@ This task delivers the canonical production VIO. The other two strategies (VINS-
- OKVIS2 upstream-source modifications — upstream HEAD is pinned per Plan-phase; deviations require an explicit ADR.
- Multi-camera OKVIS2 — out of scope (single nav-camera per RESTRICT-UAV-3).
## Implementation Notes (2026-05-12, batch 23)
Carry-over plan (`_docs/03_implementation/AZ-332_implementation_plan.md`) splits AZ-332 into:
1. **This batch** — production-quality Python facade (`okvis2.py`), `Okvis2Config` schema extension, FDR `vio.health` kind, full AC-1..8 + AC-10 coverage against a `FakeOkvis2Backend` fixture (`tests/unit/c1_vio/conftest.py`), pybind11 binding source that compiles + loads but throws `OkvisFatalException("estimator not yet wired")` on first `add_frame` (loud-fail, never silent), CMake glue at `cpp/okvis2/CMakeLists.txt` (gated by `BUILD_OKVIS2`).
2. **Tier-2 follow-up** — actual `okvis::ThreadedKFVio` wiring inside the binding, CI matrix that installs Ceres + initialises OKVIS2's vendored submodules, AC-9 + NFR-perf validation on Jetson against Derkachi-class fixtures. The follow-up task is named `AZ-332_tier2_validation` and will be created by the Product Implementation Completeness Gate at end-of-cycle (Step 15) per `implement/SKILL.md`. Until that lands, GitHub Actions Linux CI builds with `-DBUILD_OKVIS2=OFF` (see `.github/workflows/ci.yml` comment).
Constructor signature contradiction (task-spec vs AZ-331 factory) resolved 2026-05-12 in favour of the factory: `__init__(self, config: Config, *, fdr_client: FdrClient, clock: Clock | None = None)`. Calibration / preintegrator / logger are built internally from `config`. No churn on AZ-331's already-tested factory.
IMU-substrate contradiction (task-spec "MUST consume IMU via AZ-276 ImuPreintegrator" vs OKVIS2's internal IMU integration owned by `okvis::ThreadedKFVio`) resolved 2026-05-12: OKVIS2 owns its own IMU integration for the VIO estimator's keyframe factor; the AZ-276 preintegrator is a *separate* substrate consumed by E-C5's fusion graph. The "single source of IMU truth" invariant operates at the *sample-stream* level (one IMU producer), not at the integrator-instance level.
FDR API surface (`FdrClient.emit` in original prose) resolved to the actual public method `FdrClient.enqueue(record)`.
## Acceptance Criteria
**AC-1: `current_strategy_label()` returns `"okvis2"`**