azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 23:01:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

[AZ-334] C1 KLT/RANSAC strategy — engine-rule simple-baseline VIO

Browse Source 
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
2026-05-14 02:40:01 +03:00
parent 4815dd6aa1
commit ceb24b5a62
10 changed files with 2371 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/components/c1_vio/__init__.py
+2
View File
@@ -27,6 +27,7 @@ from gps_denied_onboard._types.nav import (
)
from gps_denied_onboard.components.c1_vio.config import (
C1VioConfig,
KltRansacConfig,
Okvis2Config,
VinsMonoConfig,
)
@@ -44,6 +45,7 @@ register_component_block("c1_vio", C1VioConfig)
__all__ = [
"C1VioConfig",
"FeatureQuality",
"KltRansacConfig",
"Okvis2Config",
"VinsMonoConfig",
"VioDegradedError",
src/gps_denied_onboard/components/c1_vio/config.py
+90 -1
View File
@@ -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
``__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,
degraded-feature threshold, per-frame debug log). Only consulted when
``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
@@ -29,6 +36,7 @@ from gps_denied_onboard.config.schema import ConfigError
__all__ = [
"KNOWN_STRATEGIES",
"C1VioConfig",
"KltRansacConfig",
"Okvis2Config",
"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)
class C1VioConfig:
"""Per-component config for C1 VIO.
@@ -195,6 +280,9 @@ class C1VioConfig:
``vins_mono`` carries VINS-Mono-specific knobs (AZ-333); consulted
only when ``strategy == "vins_mono"``.
``klt_ransac`` carries KLT/RANSAC-specific knobs (AZ-334);
consulted only when ``strategy == "klt_ransac"``.
"""
strategy: str = "klt_ransac"
@@ -202,6 +290,7 @@ class C1VioConfig:
warm_start_max_frames: int = 5
okvis2: Okvis2Config = field(default_factory=Okvis2Config)
vins_mono: VinsMonoConfig = field(default_factory=VinsMonoConfig)
klt_ransac: KltRansacConfig = field(default_factory=KltRansacConfig)
def __post_init__(self) -> None:
if self.strategy not in KNOWN_STRATEGIES:
src/gps_denied_onboard/components/c1_vio/klt_ransac.py
+769
View File
@@ -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)