mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 17:41:13 +00:00
[AZ-528] Consolidate c1_vio strategy facade orchestration spine
Replace 3-way byte-equivalent orchestration-spine duplication across okvis2.py / vins_mono.py / klt_ransac.py with a single c1-internal helper at components/c1_vio/_facade_spine.py. Closes cumulative review batches 52-54 Finding F1. No behaviour change — all existing AZ-332 / AZ-333 / AZ-334 AC tests pass unmodified (114 c1_vio tests green, 237 with adjacent regression suite). The helper exposes 5 stateless free functions (now_iso, bias_norm, se3_from_4x4, frame_ts_ns, frame_image) and a FacadeSpine mixin class providing _classify_state / _tick_lost / _emit_transition. Concrete strategies inherit the mixin and set spine-required instance attributes in __init__. Mirrors the AZ-527 precedent for c2_vpr-side _assert_engine_output_dim consolidation. New test file test_az528_facade_spine.py covers AC-1..AC-8 with 19 tests, including an AST regression guard that prevents future re-introduction of the consolidated free functions in any strategy module, plus a Risk-1 static check that every strategy's __init__ assigns every spine-required attribute. Archive AZ-528 task spec to done/, bump autodev state to batch 56. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,224 @@
|
||||
"""c1_vio strategy facade orchestration spine — c1-internal helpers (AZ-528).
|
||||
|
||||
Shared between :class:`Okvis2Strategy` (AZ-332),
|
||||
:class:`VinsMonoStrategy` (AZ-333), and :class:`KltRansacStrategy`
|
||||
(AZ-334). Closes cumulative review batches 52-54 Finding F1: the
|
||||
orchestration-spine duplication across the c1_vio strategy triplet.
|
||||
|
||||
Mirrors the AZ-527 precedent for the c2_vpr-side consolidation —
|
||||
underscore-prefixed module name keeps the helper c1-internal (NOT in
|
||||
``c1_vio/__init__.py``'s Public API surface); concrete strategies
|
||||
import it as a sibling. Engine output-shape contracts and VIO state
|
||||
machines are component-scoped concerns, NOT cross-component
|
||||
``shared/helpers/`` concerns.
|
||||
|
||||
The helper module exposes:
|
||||
|
||||
Free functions (stateless, pure):
|
||||
|
||||
- :func:`now_iso` — ISO-8601 UTC timestamp for FDR record ``ts``.
|
||||
- :func:`bias_norm` — L2 norm of the concatenated ``(accel || gyro)``
|
||||
6-vector.
|
||||
- :func:`se3_from_4x4` — lazy ``gtsam.Pose3`` construction from a
|
||||
4x4 row-major matrix.
|
||||
- :func:`frame_ts_ns` — UTC-epoch nanoseconds from
|
||||
:class:`NavCameraFrame` (OKVIS2 + VINS-Mono only; KLT/RANSAC owns
|
||||
inline grayscale conversion via its geometry pipeline).
|
||||
- :func:`frame_image` — contiguous ``uint8`` ndarray with 2D/3D
|
||||
shape validation. ``producer_id`` is parametrised so the error
|
||||
message names the originating strategy.
|
||||
|
||||
Mixin (:class:`FacadeSpine`) provides the state-machine + FDR
|
||||
``vio.health`` record emit. Concrete strategies inherit from it
|
||||
and set the per-instance attributes the mixin reads:
|
||||
|
||||
- ``_reported_state``, ``_frames_since_warmup``,
|
||||
``_warm_start_max_frames``, ``_feature_threshold`` — used by
|
||||
:meth:`FacadeSpine._classify_state`.
|
||||
- ``_consecutive_lost``, ``_lost_frame_threshold`` — used by
|
||||
:meth:`FacadeSpine._tick_lost`.
|
||||
- ``_last_emitted_state``, ``_producer_id``, ``_strategy_label``,
|
||||
``_latest_bias``, ``_fdr`` — used by
|
||||
:meth:`FacadeSpine._emit_transition`.
|
||||
|
||||
The mixin's required attributes are declared as class-level type
|
||||
annotations (no default values) so type-checkers / IDEs catch a
|
||||
strategy that forgets to set one in ``__init__`` — runtime safety
|
||||
is the AZ-528 AC-9 + AC-10 regression-guard tests that exercise
|
||||
the three concrete strategies end-to-end through the consolidated
|
||||
spine.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from datetime import datetime, timezone
|
||||
from typing import TYPE_CHECKING, Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from gps_denied_onboard._types.nav import (
|
||||
FeatureQuality,
|
||||
ImuBias,
|
||||
VioState,
|
||||
)
|
||||
from gps_denied_onboard.components.c1_vio.errors import VioFatalError
|
||||
from gps_denied_onboard.fdr_client.records import CURRENT_SCHEMA_VERSION, FdrRecord
|
||||
|
||||
if TYPE_CHECKING:
|
||||
import numpy.typing as npt
|
||||
|
||||
from gps_denied_onboard._types.nav import NavCameraFrame
|
||||
from gps_denied_onboard.fdr_client.client import FdrClient
|
||||
|
||||
|
||||
__all__ = [
|
||||
"FacadeSpine",
|
||||
"bias_norm",
|
||||
"frame_image",
|
||||
"frame_ts_ns",
|
||||
"now_iso",
|
||||
"se3_from_4x4",
|
||||
]
|
||||
|
||||
|
||||
def now_iso() -> str:
|
||||
"""ISO-8601 UTC timestamp for FDR record ``ts``.
|
||||
|
||||
Returns the format used by all three c1_vio strategies' FDR
|
||||
record emissions (``+00:00`` offset suffix). This is NOT the
|
||||
``Z``-suffix canonical form used by
|
||||
:func:`gps_denied_onboard.helpers.iso_timestamps.iso_ts_from_clock`
|
||||
(AZ-526) — that helper serves clock-injected FDR timestamps
|
||||
from a :class:`~gps_denied_onboard.clock.Clock`, while this
|
||||
helper is the strategy-facade's own wall-clock stamp at state-
|
||||
transition emit time.
|
||||
"""
|
||||
return datetime.now(timezone.utc).isoformat()
|
||||
|
||||
|
||||
def bias_norm(bias: ImuBias) -> float:
|
||||
"""L2 norm of the concatenated ``(accel_bias || gyro_bias)`` 6-vector."""
|
||||
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 frame_ts_ns(frame: NavCameraFrame) -> int:
|
||||
"""Convert :attr:`NavCameraFrame.timestamp` to monotonic ns.
|
||||
|
||||
Uses the datetime's UTC epoch nanoseconds so the value is
|
||||
monotonically increasing across frames (frame source guarantees
|
||||
strictly increasing timestamps per the FrameSource contract).
|
||||
"""
|
||||
return int(frame.timestamp.timestamp() * 1e9)
|
||||
|
||||
|
||||
def frame_image(frame: NavCameraFrame, *, producer_id: str) -> np.ndarray:
|
||||
"""Coerce the frame's image into a contiguous ``uint8`` ndarray.
|
||||
|
||||
Raises :class:`VioFatalError` tagged with ``producer_id`` when
|
||||
the frame's image is not 2-D or 3-D.
|
||||
"""
|
||||
arr = np.ascontiguousarray(frame.image, dtype=np.uint8)
|
||||
if arr.ndim < 2 or arr.ndim > 3:
|
||||
raise VioFatalError(
|
||||
f"{producer_id}: NavCameraFrame.image must be 2-D or 3-D; got {arr.ndim}-D"
|
||||
)
|
||||
return arr
|
||||
|
||||
|
||||
class FacadeSpine:
|
||||
"""Orchestration-spine mixin for c1_vio :class:`VioStrategy` implementations.
|
||||
|
||||
Implements three state-machine helpers that were previously duplicated
|
||||
across :class:`Okvis2Strategy`, :class:`VinsMonoStrategy`, and
|
||||
:class:`KltRansacStrategy`:
|
||||
|
||||
- :meth:`_classify_state` — INIT during warmup, DEGRADED below
|
||||
the per-strategy feature threshold, TRACKING otherwise.
|
||||
- :meth:`_tick_lost` — increment the consecutive-lost counter,
|
||||
escalate to LOST at ``_lost_frame_threshold``, demote
|
||||
TRACKING → DEGRADED on the first lost frame.
|
||||
- :meth:`_emit_transition` — emit exactly one FDR ``vio.health``
|
||||
record per state change; no record on steady-state.
|
||||
|
||||
Concrete strategies MUST set the following attributes (typically
|
||||
in ``__init__``) before any of the mixin methods are called:
|
||||
|
||||
- ``_reported_state: VioState``
|
||||
- ``_frames_since_warmup: int``
|
||||
- ``_warm_start_max_frames: int``
|
||||
- ``_feature_threshold: int``
|
||||
- ``_consecutive_lost: int``
|
||||
- ``_lost_frame_threshold: int``
|
||||
- ``_last_emitted_state: VioState | None``
|
||||
- ``_producer_id: str`` — FDR record ``producer_id`` field
|
||||
(e.g. ``"c1_vio.okvis2"``).
|
||||
- ``_strategy_label: str`` — FDR payload ``strategy_label``
|
||||
field (e.g. ``"okvis2"``).
|
||||
- ``_latest_bias: ImuBias``
|
||||
- ``_fdr: FdrClient``
|
||||
|
||||
Type annotations on the class declare the attributes for static
|
||||
type-checkers; missing assignments at runtime surface as
|
||||
``AttributeError`` at the first state-machine call site.
|
||||
"""
|
||||
|
||||
_reported_state: VioState
|
||||
_frames_since_warmup: int
|
||||
_warm_start_max_frames: int
|
||||
_feature_threshold: int
|
||||
_consecutive_lost: int
|
||||
_lost_frame_threshold: int
|
||||
_last_emitted_state: VioState | None
|
||||
_producer_id: str
|
||||
_strategy_label: str
|
||||
_latest_bias: ImuBias
|
||||
_fdr: FdrClient
|
||||
|
||||
def _classify_state(self, fq: FeatureQuality) -> 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._feature_threshold:
|
||||
return VioState.DEGRADED
|
||||
return VioState.TRACKING
|
||||
|
||||
def _tick_lost(self, frame_id: str) -> None:
|
||||
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:
|
||||
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=self._producer_id,
|
||||
kind="vio.health",
|
||||
payload={
|
||||
"state": new_state.value,
|
||||
"consecutive_lost": self._consecutive_lost,
|
||||
"bias_norm": bias_norm(self._latest_bias),
|
||||
"strategy_label": self._strategy_label,
|
||||
"frame_id": frame_id,
|
||||
},
|
||||
)
|
||||
self._fdr.enqueue(record)
|
||||
@@ -74,8 +74,6 @@ Risk mitigations (see task spec for full text):
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from datetime import datetime, timezone
|
||||
from typing import TYPE_CHECKING, Any, Final, Literal
|
||||
|
||||
import cv2
|
||||
@@ -89,11 +87,15 @@ from gps_denied_onboard._types.nav import (
|
||||
VioState,
|
||||
)
|
||||
from gps_denied_onboard.clock.wall_clock import WallClock
|
||||
from gps_denied_onboard.components.c1_vio._facade_spine import (
|
||||
FacadeSpine,
|
||||
bias_norm,
|
||||
se3_from_4x4,
|
||||
)
|
||||
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,
|
||||
@@ -133,28 +135,6 @@ _ESSENTIAL_MATRIX_DOF: Final[int] = 5
|
||||
_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``.
|
||||
|
||||
@@ -204,7 +184,7 @@ def _intrinsics_3x3(calibration: CameraCalibration) -> np.ndarray:
|
||||
return K
|
||||
|
||||
|
||||
class KltRansacStrategy:
|
||||
class KltRansacStrategy(FacadeSpine):
|
||||
"""Mandatory simple-baseline :class:`VioStrategy` for E-C1 (AZ-334).
|
||||
|
||||
Constructor matches the AZ-331 composition-root factory shape::
|
||||
@@ -244,6 +224,9 @@ class KltRansacStrategy:
|
||||
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
|
||||
self._feature_threshold: int = self._cfg.min_features_for_pose
|
||||
self._producer_id: str = _PRODUCER_ID
|
||||
self._strategy_label: str = _STRATEGY_LABEL
|
||||
|
||||
# Per-frame state.
|
||||
self._prev_gray: np.ndarray | None = None
|
||||
@@ -404,7 +387,7 @@ class KltRansacStrategy:
|
||||
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)
|
||||
pose = se3_from_4x4(pose_4x4)
|
||||
|
||||
# 8. Final inlier count for state classification + covariance.
|
||||
final_inlier_count = (
|
||||
@@ -529,7 +512,7 @@ class KltRansacStrategy:
|
||||
return VioHealth(
|
||||
state=self._reported_state,
|
||||
consecutive_lost=self._consecutive_lost,
|
||||
bias_norm=_bias_norm(self._latest_bias),
|
||||
bias_norm=bias_norm(self._latest_bias),
|
||||
)
|
||||
|
||||
def current_strategy_label(self) -> Literal["okvis2", "vins_mono", "klt_ransac"]:
|
||||
@@ -614,7 +597,7 @@ class KltRansacStrategy:
|
||||
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))
|
||||
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,
|
||||
@@ -715,52 +698,6 @@ class KltRansacStrategy:
|
||||
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."""
|
||||
|
||||
@@ -44,8 +44,6 @@ AC mapping (see ``_docs/02_tasks/todo/AZ-332_c1_okvis2_strategy.md``):
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from datetime import datetime, timezone
|
||||
from typing import TYPE_CHECKING, Any, Final, Literal
|
||||
|
||||
import numpy as np
|
||||
@@ -58,16 +56,20 @@ from gps_denied_onboard._types.nav import (
|
||||
VioState,
|
||||
)
|
||||
from gps_denied_onboard.clock.wall_clock import WallClock
|
||||
from gps_denied_onboard.components.c1_vio._facade_spine import (
|
||||
FacadeSpine,
|
||||
bias_norm,
|
||||
frame_image,
|
||||
frame_ts_ns,
|
||||
se3_from_4x4,
|
||||
)
|
||||
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.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,
|
||||
@@ -85,32 +87,9 @@ __all__ = ["Okvis2Strategy"]
|
||||
_STRATEGY_LABEL: Final[Literal["okvis2"]] = "okvis2"
|
||||
_PRODUCER_ID: Final[str] = "c1_vio.okvis2"
|
||||
_LOGGER_COMPONENT: Final[str] = "c1_vio.okvis2"
|
||||
_BIAS_NORM_FLOOR: Final[float] = 0.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))
|
||||
|
||||
|
||||
class Okvis2Strategy:
|
||||
class Okvis2Strategy(FacadeSpine):
|
||||
"""Production-default :class:`VioStrategy` for E-C1 (AZ-332).
|
||||
|
||||
Constructor matches the AZ-331 composition-root factory shape::
|
||||
@@ -147,6 +126,9 @@ class Okvis2Strategy:
|
||||
self._lost_frame_threshold: int = c1_block.lost_frame_threshold
|
||||
self._warm_start_max_frames: int = c1_block.warm_start_max_frames
|
||||
self._okvis2_cfg: Okvis2Config = c1_block.okvis2
|
||||
self._feature_threshold: int = self._okvis2_cfg.degraded_feature_threshold
|
||||
self._producer_id: str = _PRODUCER_ID
|
||||
self._strategy_label: str = _STRATEGY_LABEL
|
||||
self._calibration: CameraCalibration | None = None
|
||||
self._frames_since_warmup: int = 0
|
||||
self._consecutive_lost: int = 0
|
||||
@@ -202,7 +184,9 @@ class Okvis2Strategy:
|
||||
try:
|
||||
self._push_imu_window(imu)
|
||||
produced = self._backend.add_frame(
|
||||
frame_id_str, _frame_ts_ns(frame), _frame_image(frame)
|
||||
frame_id_str,
|
||||
frame_ts_ns(frame),
|
||||
frame_image(frame, producer_id="Okvis2Strategy"),
|
||||
)
|
||||
except self._binding_module.OkvisInitException as exc:
|
||||
self._emit_transition(VioState.INIT, frame_id_str)
|
||||
@@ -319,7 +303,7 @@ class Okvis2Strategy:
|
||||
return VioHealth(
|
||||
state=self._reported_state,
|
||||
consecutive_lost=self._consecutive_lost,
|
||||
bias_norm=_bias_norm(self._latest_bias),
|
||||
bias_norm=bias_norm(self._latest_bias),
|
||||
)
|
||||
|
||||
def current_strategy_label(self) -> Literal["okvis2", "vins_mono", "klt_ransac"]:
|
||||
@@ -400,7 +384,7 @@ class Okvis2Strategy:
|
||||
|
||||
def _build_vio_output(self, raw: dict[str, Any], emitted_at_ns: int) -> VioOutput:
|
||||
try:
|
||||
pose = _se3_from_4x4(raw["pose_T_world_body"])
|
||||
pose = se3_from_4x4(raw["pose_T_world_body"])
|
||||
cov = np.asarray(raw["pose_covariance_6x6"], dtype=np.float64)
|
||||
bias = ImuBias(
|
||||
accel_bias=tuple(float(x) for x in raw["accel_bias"]), # type: ignore[arg-type]
|
||||
@@ -432,57 +416,3 @@ class Okvis2Strategy:
|
||||
emitted_at_ns=backend_ts,
|
||||
)
|
||||
|
||||
def _classify_state(self, fq: FeatureQuality) -> 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._okvis2_cfg.degraded_feature_threshold:
|
||||
return VioState.DEGRADED
|
||||
return VioState.TRACKING
|
||||
|
||||
def _tick_lost(self, frame_id: str) -> None:
|
||||
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:
|
||||
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 _frame_ts_ns(frame: NavCameraFrame) -> int:
|
||||
"""Convert ``NavCameraFrame.timestamp`` to monotonic-ns.
|
||||
|
||||
Uses the datetime's UTC epoch nanoseconds so the value is
|
||||
monotonically increasing across frames (frame source guarantees
|
||||
strictly increasing timestamps per the FrameSource contract).
|
||||
"""
|
||||
return int(frame.timestamp.timestamp() * 1e9)
|
||||
|
||||
|
||||
def _frame_image(frame: NavCameraFrame) -> np.ndarray:
|
||||
"""Coerce the frame's image into a contiguous uint8 ndarray."""
|
||||
arr = np.ascontiguousarray(frame.image, dtype=np.uint8)
|
||||
if arr.ndim < 2 or arr.ndim > 3:
|
||||
raise VioFatalError(
|
||||
f"Okvis2Strategy: NavCameraFrame.image must be 2-D or 3-D; got {arr.ndim}-D"
|
||||
)
|
||||
return arr
|
||||
|
||||
@@ -53,8 +53,6 @@ AC mapping (see ``_docs/02_tasks/todo/AZ-333_c1_vins_mono_strategy.md``):
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from datetime import datetime, timezone
|
||||
from typing import TYPE_CHECKING, Any, Final, Literal
|
||||
|
||||
import numpy as np
|
||||
@@ -67,16 +65,20 @@ from gps_denied_onboard._types.nav import (
|
||||
VioState,
|
||||
)
|
||||
from gps_denied_onboard.clock.wall_clock import WallClock
|
||||
from gps_denied_onboard.components.c1_vio._facade_spine import (
|
||||
FacadeSpine,
|
||||
bias_norm,
|
||||
frame_image,
|
||||
frame_ts_ns,
|
||||
se3_from_4x4,
|
||||
)
|
||||
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.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,
|
||||
@@ -96,29 +98,7 @@ _PRODUCER_ID: Final[str] = "c1_vio.vins_mono"
|
||||
_LOGGER_COMPONENT: Final[str] = "c1_vio.vins_mono"
|
||||
|
||||
|
||||
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))
|
||||
|
||||
|
||||
class VinsMonoStrategy:
|
||||
class VinsMonoStrategy(FacadeSpine):
|
||||
"""Research-only :class:`VioStrategy` for IT-12 comparative study (AZ-333).
|
||||
|
||||
Constructor matches the AZ-331 composition-root factory shape::
|
||||
@@ -157,6 +137,9 @@ class VinsMonoStrategy:
|
||||
self._lost_frame_threshold: int = c1_block.lost_frame_threshold
|
||||
self._warm_start_max_frames: int = c1_block.warm_start_max_frames
|
||||
self._vins_cfg: VinsMonoConfig = c1_block.vins_mono
|
||||
self._feature_threshold: int = self._vins_cfg.degraded_feature_threshold
|
||||
self._producer_id: str = _PRODUCER_ID
|
||||
self._strategy_label: str = _STRATEGY_LABEL
|
||||
self._calibration: CameraCalibration | None = None
|
||||
self._frames_since_warmup: int = 0
|
||||
self._consecutive_lost: int = 0
|
||||
@@ -216,7 +199,9 @@ class VinsMonoStrategy:
|
||||
try:
|
||||
self._push_imu_window(imu)
|
||||
produced = self._backend.add_frame(
|
||||
frame_id_str, _frame_ts_ns(frame), _frame_image(frame)
|
||||
frame_id_str,
|
||||
frame_ts_ns(frame),
|
||||
frame_image(frame, producer_id="VinsMonoStrategy"),
|
||||
)
|
||||
except self._binding_module.VinsMonoInitException as exc:
|
||||
self._emit_transition(VioState.INIT, frame_id_str)
|
||||
@@ -338,7 +323,7 @@ class VinsMonoStrategy:
|
||||
return VioHealth(
|
||||
state=self._reported_state,
|
||||
consecutive_lost=self._consecutive_lost,
|
||||
bias_norm=_bias_norm(self._latest_bias),
|
||||
bias_norm=bias_norm(self._latest_bias),
|
||||
)
|
||||
|
||||
def current_strategy_label(self) -> Literal["okvis2", "vins_mono", "klt_ransac"]:
|
||||
@@ -426,7 +411,7 @@ class VinsMonoStrategy:
|
||||
|
||||
def _build_vio_output(self, raw: dict[str, Any], emitted_at_ns: int) -> VioOutput:
|
||||
try:
|
||||
pose = _se3_from_4x4(raw["pose_T_world_body"])
|
||||
pose = se3_from_4x4(raw["pose_T_world_body"])
|
||||
cov = np.asarray(raw["pose_covariance_6x6"], dtype=np.float64)
|
||||
bias = ImuBias(
|
||||
accel_bias=tuple(float(x) for x in raw["accel_bias"]), # type: ignore[arg-type]
|
||||
@@ -461,58 +446,3 @@ class VinsMonoStrategy:
|
||||
emitted_at_ns=backend_ts,
|
||||
)
|
||||
|
||||
def _classify_state(self, fq: FeatureQuality) -> 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._vins_cfg.degraded_feature_threshold:
|
||||
return VioState.DEGRADED
|
||||
return VioState.TRACKING
|
||||
|
||||
def _tick_lost(self, frame_id: str) -> None:
|
||||
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:
|
||||
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 _frame_ts_ns(frame: NavCameraFrame) -> int:
|
||||
"""Convert ``NavCameraFrame.timestamp`` to monotonic-ns.
|
||||
|
||||
Uses the datetime's UTC epoch nanoseconds so the value is
|
||||
monotonically increasing across frames (frame source guarantees
|
||||
strictly increasing timestamps per the FrameSource contract).
|
||||
"""
|
||||
return int(frame.timestamp.timestamp() * 1e9)
|
||||
|
||||
|
||||
def _frame_image(frame: NavCameraFrame) -> np.ndarray:
|
||||
"""Coerce the frame's image into a contiguous uint8 ndarray."""
|
||||
arr = np.ascontiguousarray(frame.image, dtype=np.uint8)
|
||||
if arr.ndim < 2 or arr.ndim > 3:
|
||||
raise VioFatalError(
|
||||
f"VinsMonoStrategy: NavCameraFrame.image must be 2-D or 3-D; "
|
||||
f"got {arr.ndim}-D"
|
||||
)
|
||||
return arr
|
||||
|
||||
Reference in New Issue
Block a user