[AZ-331] C1 VioStrategy: Protocol + DTOs + factory + C5 migration

Freezes the c1_vio Public API per
_docs/02_document/contracts/c1_vio/vio_strategy_protocol.md v1.0.0:

- VioStrategy Protocol (4 methods: process_frame, reset_to_warm_start,
  health_snapshot, current_strategy_label) in
  components/c1_vio/interface.py.
- DTOs (VioOutput, VioHealth, FeatureQuality, WarmStartPose) + VioState
  enum in _types/nav.py — L1 placement so C5 + C13 consume them without
  crossing the components.* boundary (AZ-270 AC-6). The new VioOutput
  shape (frame_id: str, relative_pose_T: gtsam.Pose3,
  pose_covariance_6x6, imu_bias, feature_quality, emitted_at_ns)
  replaces the AZ-263 scaffolding in _types/vio.py, which is now
  deleted.
- VioError family (VioInitializingError / VioDegradedError /
  VioFatalError) in components/c1_vio/errors.py. Documented
  rationale: the degraded-operation path returns a VioOutput with
  inflated covariance + VioHealth.state=DEGRADED rather than raising
  VioDegradedError — the error type exists only for the rare
  degraded->fatal transition.
- C1VioConfig per-component config block (strategy enum,
  lost_frame_threshold default 9, warm_start_max_frames default 5)
  with constructor-time validation rejecting unknown strategy labels.
- StrategyNotAvailableError added to runtime_root/errors.py;
  composition-time error distinct from the VioError family.
- Composition-root factory build_vio_strategy in
  runtime_root/vio_factory.py with three BUILD_* gates (BUILD_OKVIS2,
  BUILD_VINS_MONO, BUILD_KLT_RANSAC). Concrete strategy modules are
  imported lazily via __import__ AFTER the flag check — Tier-0
  workstation builds with the flag OFF MUST NOT load the strategy
  module (Risk-2 / I-5; verifiable via sys.modules).
- 36 conformance tests cover all 9 ACs + NFR-perf-factory
  (p99 build under 200 ms x 1000 calls) + NFR-reliability-error-family.
  AC-8 introspects the contract file's Shape table and asserts method
  parity against the runtime Protocol; AC-9 asserts the frame_id
  annotation is 'str' (PEP-563 stringified).

C5 migration (consumers of the new VioOutput shape):
- gtsam_isam2_estimator.py + eskf_baseline.py: replaced
  vio.timestamp -> vio.emitted_at_ns (drops _datetime_to_ns on the
  VIO path), vio.pose_se3 -> vio.relative_pose_T (gtsam.Pose3 direct;
  drops _pose_se3_to_gtsam / _pose_se3_to_array), vio.covariance_6x6
  -> vio.pose_covariance_6x6 (rename).
- key_for_frame signature widened to UUID | int | str to accept the
  new str frame_id.
- 4 C5 test files migrated to the new VioOutput shape with helper
  fixtures producing ImuBias + FeatureQuality + str frame_id.
- c5_state/interface.py TYPE_CHECKING import path updated.

Bootstrap healthcheck + test_types_importable updated to drop the
deleted _types/vio module and pick up _types/inference (AZ-297) in
the same sweep.

Full unit-test sweep: 884 passed, 2 pre-existing environment skips
(cmake, actionlint).

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-12 04:44:31 +03:00
parent daff5d4d1c
commit 6c7d24f7e0
20 changed files with 1027 additions and 81 deletions
+20 -6
View File
@@ -32,14 +32,19 @@ import numpy as np
import pytest
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.nav import ImuSample, ImuWindow
from gps_denied_onboard._types.nav import (
FeatureQuality,
ImuBias,
ImuSample,
ImuWindow,
VioOutput,
)
from gps_denied_onboard._types.pose import (
CovarianceMode,
PoseEstimate,
PoseSourceLabel,
Quat,
)
from gps_denied_onboard._types.vio import VioOutput
from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.errors import EstimatorDegradedError
from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import (
@@ -73,12 +78,21 @@ def _build_estimator(*, with_stub_handle: bool = True) -> GtsamIsam2StateEstimat
return estimator
_ZERO_BIAS = ImuBias(accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0))
_NEUTRAL_FQ = FeatureQuality(
tracked=20, new=2, lost=1, mean_parallax=5.0, mre_px=1.0,
)
def _make_vio(*, frame_id: int, t_seconds: float, pose: np.ndarray | None = None) -> VioOutput:
matrix = pose if pose is not None else np.eye(4)
return VioOutput(
frame_id=frame_id,
timestamp=datetime.fromtimestamp(t_seconds, tz=timezone.utc),
pose_se3=pose if pose is not None else np.eye(4),
covariance_6x6=np.eye(6) * 0.01,
frame_id=str(frame_id),
relative_pose_T=gtsam.Pose3(matrix),
pose_covariance_6x6=np.eye(6) * 0.01,
imu_bias=_ZERO_BIAS,
feature_quality=_NEUTRAL_FQ,
emitted_at_ns=int(t_seconds * 1_000_000_000),
)