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

[AZ-623] [AZ-625] Phase E: c282_ransac + c5 helpers; split handle work

Browse Source 
Wire 4 stateless / cached helpers into airborne_bootstrap.build_pre_constructed:
c282_ransac_filter, c5_imu_preintegrator (cached on calibration path),
c5_se3_utils (helpers.se3_utils module as namespace handle), c5_wgs_converter.

The original AZ-623 5th deliverable (c5_isam2_graph_handle) hit an
unresolvable construction-order conflict between c4_pose (consumes the handle)
and c5_state (creates it inside build_state_estimator's tuple return) under
the umbrella's "MUST NOT touch any per-component factory signature" constraint.
Per AZ-623 spec's escalation gate, scope was split: AZ-625 captures the handle
ordering work; AZ-624 dependency edge updated to require both.

Tests: tests/unit/runtime_root/test_az623_pre_constructed_phase_e.py adds 7
tests covering AC-623.1..3 (4 new keys + correct types, IMU preintegrator
caching, operator-actionable error messages for empty / unreadable / malformed
calibration paths). Autouse stubs added to test_az619/620/621/622 so prior
phase tests remain isolated from new builders.

Quality gates: ruff format clean, ruff lint clean, 24/24 phase tests pass,
247/247 runtime_root + c5_state regression suite passes. Code review verdict
PASS_WITH_WARNINGS (3 Low findings; full report in
_docs/03_implementation/reviews/batch_94_review.md).

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-19 09:20:28 +03:00
parent 5c4d129f80
commit 02208c577e
13 changed files with 1014 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/runtime_root/airborne_bootstrap.py
+263 -56
View File
@@ -48,15 +48,24 @@ internals).
from __future__ import annotations
import json
import logging
import os
from collections.abc import Mapping
from pathlib import Path
from typing import TYPE_CHECKING, Any, Final
from gps_denied_onboard._types.calibration import CameraCalibration
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.fdr_client.client import make_fdr_client
from gps_denied_onboard.helpers.feature_extractor import OpenCvOrbExtractor
from gps_denied_onboard.helpers.imu_preintegrator import (
ImuPreintegrator,
make_imu_preintegrator,
)
from gps_denied_onboard.helpers.lightglue_runtime import LightGlueRuntime
from gps_denied_onboard.helpers.ransac_filter import RansacFilter
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
from gps_denied_onboard.runtime_root import register_strategy
from gps_denied_onboard.runtime_root.errors import RuntimeNotAvailableError
from gps_denied_onboard.runtime_root.inference_factory import build_inference_runtime
@@ -86,10 +95,36 @@ __all__ = [
"FAISS_BUILD_FLAG",
"AirborneBootstrapError",
"build_pre_constructed",
"clear_imu_preintegrator_cache",
"register_airborne_strategies",
]
_IMU_PREINTEGRATOR_CACHE: dict[str, ImuPreintegrator] = {}
"""Per-process cache mapping ``camera_calibration_path`` to the
:class:`ImuPreintegrator` built for that path.
Backs AC-623.2: invoking :func:`build_pre_constructed` twice in the
same process MUST return the SAME ``c5_imu_preintegrator`` instance
when the calibration path is unchanged. The preintegrator is the only
stateful c5 helper this phase wires; caching protects its bias /
sample accumulator from being silently rebuilt on a re-invocation.
Tests call :func:`clear_imu_preintegrator_cache` to isolate state.
"""
def clear_imu_preintegrator_cache() -> None:
"""Drop every cached :class:`ImuPreintegrator` (test-isolation only).
Mirrors :func:`gps_denied_onboard.fdr_client.client.clear_fdr_client_cache` /
:func:`gps_denied_onboard.runtime_root.state_factory.clear_state_registry`'s
test-only contract: production code never calls this, but unit tests
that exercise the per-path cache need a way to reset between cases.
"""
_IMU_PREINTEGRATOR_CACHE.clear()
FAISS_BUILD_FLAG: Final[str] = "BUILD_FAISS_INDEX"
"""Env flag gating the FAISS-backed ``DescriptorIndex`` impl.
@@ -229,9 +264,7 @@ _C4_POSE_STRATEGIES: tuple[str, ...] = ("opencv_gtsam",)
_C5_STATE_STRATEGIES: tuple[str, ...] = ("gtsam_isam2", "eskf")
def _require(
constructed: Mapping[str, Any], key: str, component_slug: str
) -> Any:
def _require(constructed: Mapping[str, Any], key: str, component_slug: str) -> Any:
"""Extract ``constructed[key]`` or raise AirborneBootstrapError."""
if key not in constructed:
available = sorted(constructed.keys())
@@ -262,16 +295,10 @@ def _c2_vpr_wrapper(config: Config, constructed: Mapping[str, Any]) -> Any:
)
def _c2_5_rerank_wrapper(
config: Config, constructed: Mapping[str, Any]
) -> Any:
def _c2_5_rerank_wrapper(config: Config, constructed: Mapping[str, Any]) -> Any:
tile_store = _require(constructed, "c6_tile_store", "c2_5_rerank")
lightglue_runtime = _require(
constructed, "c3_lightglue_runtime", "c2_5_rerank"
)
feature_extractor = _require(
constructed, "c3_feature_extractor", "c2_5_rerank"
)
lightglue_runtime = _require(constructed, "c3_lightglue_runtime", "c2_5_rerank")
feature_extractor = _require(constructed, "c3_feature_extractor", "c2_5_rerank")
clock = _require(constructed, "clock", "c2_5_rerank")
fdr_client = constructed.get("c13_fdr")
return build_rerank_strategy(
@@ -284,12 +311,8 @@ def _c2_5_rerank_wrapper(
)
def _c3_matcher_wrapper(
config: Config, constructed: Mapping[str, Any]
) -> Any:
lightglue_runtime = _require(
constructed, "c3_lightglue_runtime", "c3_matcher"
)
def _c3_matcher_wrapper(config: Config, constructed: Mapping[str, Any]) -> Any:
lightglue_runtime = _require(constructed, "c3_lightglue_runtime", "c3_matcher")
ransac_filter = _require(constructed, "c282_ransac_filter", "c3_matcher")
inference_runtime = _require(constructed, "c7_inference", "c3_matcher")
clock = constructed.get("clock")
@@ -304,9 +327,7 @@ def _c3_matcher_wrapper(
)
def _c3_5_adhop_wrapper(
config: Config, constructed: Mapping[str, Any]
) -> Any:
def _c3_5_adhop_wrapper(config: Config, constructed: Mapping[str, Any]) -> Any:
ransac_filter = _require(constructed, "c282_ransac_filter", "c3_5_adhop")
inference_runtime = _require(constructed, "c7_inference", "c3_5_adhop")
clock = constructed.get("clock")
@@ -324,9 +345,7 @@ def _c4_pose_wrapper(config: Config, constructed: Mapping[str, Any]) -> Any:
ransac_filter = _require(constructed, "c282_ransac_filter", "c4_pose")
wgs_converter = _require(constructed, "c5_wgs_converter", "c4_pose")
se3_utils = _require(constructed, "c5_se3_utils", "c4_pose")
isam2_graph_handle = _require(
constructed, "c5_isam2_graph_handle", "c4_pose"
)
isam2_graph_handle = _require(constructed, "c5_isam2_graph_handle", "c4_pose")
fdr_client = constructed.get("c13_fdr")
clock = constructed.get("clock")
return build_pose_estimator(
@@ -341,9 +360,7 @@ def _c4_pose_wrapper(config: Config, constructed: Mapping[str, Any]) -> Any:
def _c5_state_wrapper(config: Config, constructed: Mapping[str, Any]) -> Any:
imu_preintegrator = _require(
constructed, "c5_imu_preintegrator", "c5_state"
)
imu_preintegrator = _require(constructed, "c5_imu_preintegrator", "c5_state")
se3_utils = _require(constructed, "c5_se3_utils", "c5_state")
wgs_converter = _require(constructed, "c5_wgs_converter", "c5_state")
fdr_client = _require(constructed, "c13_fdr", "c5_state")
@@ -378,9 +395,7 @@ def _ensure_state_strategy_registered(config: Config) -> None:
block = getattr(config, "components", None) or {}
c5_block = block.get("c5_state") if isinstance(block, dict) else None
strategy = (
getattr(c5_block, "strategy", "gtsam_isam2")
if c5_block is not None
else "gtsam_isam2"
getattr(c5_block, "strategy", "gtsam_isam2") if c5_block is not None else "gtsam_isam2"
)
# state_factory._STATE_BUILD_FLAGS: gtsam_isam2 defaults ON-when-unset;
# eskf defaults OFF-when-unset (mirror state_factory's own logic).
@@ -420,9 +435,7 @@ descriptor_index, etc.) come from ``pre_constructed``.
"""
_AIRBORNE_REGISTRATIONS: tuple[
tuple[str, tuple[str, ...], Any, tuple[str, ...]], ...
] = (
_AIRBORNE_REGISTRATIONS: tuple[tuple[str, tuple[str, ...], Any, tuple[str, ...]], ...] = (
("c1_vio", _C1_VIO_STRATEGIES, _c1_vio_wrapper, _C1_VIO_DEPENDS_ON),
("c2_vpr", _C2_VPR_STRATEGIES, _c2_vpr_wrapper, _C2_VPR_DEPENDS_ON),
(
@@ -468,9 +481,7 @@ def _consumers_of_pre_constructed_key(key: str) -> tuple[str, ...]:
)
def _configured_consumers_of_pre_constructed_key(
config: Config, key: str
) -> tuple[str, ...]:
def _configured_consumers_of_pre_constructed_key(config: Config, key: str) -> tuple[str, ...]:
"""Return consumers of ``key`` that are present in ``config.components``.
Used to narrow an error message from "every theoretical consumer of
@@ -503,9 +514,7 @@ def _build_c6_descriptor_index(config: Config) -> Any:
try:
return build_descriptor_index(config)
except RuntimeNotAvailableError as exc:
consumers = _configured_consumers_of_pre_constructed_key(
config, "c6_descriptor_index"
)
consumers = _configured_consumers_of_pre_constructed_key(config, "c6_descriptor_index")
raise AirborneBootstrapError(
f"airborne_bootstrap: cannot construct "
f"pre_constructed['c6_descriptor_index'] because "
@@ -558,12 +567,9 @@ def _build_c7_inference(config: Config) -> Any:
try:
return build_inference_runtime(config)
except RuntimeNotAvailableError as exc:
consumers = _configured_consumers_of_pre_constructed_key(
config, "c7_inference"
)
consumers = _configured_consumers_of_pre_constructed_key(config, "c7_inference")
flag_options = ", ".join(
f"{flag}=ON for runtime {runtime!r}"
for runtime, flag in C7_AIRBORNE_BUILD_FLAGS
f"{flag}=ON for runtime {runtime!r}" for runtime, flag in C7_AIRBORNE_BUILD_FLAGS
)
raise AirborneBootstrapError(
f"airborne_bootstrap: cannot construct "
@@ -630,9 +636,7 @@ def _load_lightglue_engine_handle(
into an :class:`AirborneBootstrapError`).
"""
block = config.components.get("c3_matcher")
weights_path = (
getattr(block, "lightglue_weights_path", None) if block is not None else None
)
weights_path = getattr(block, "lightglue_weights_path", None) if block is not None else None
if weights_path is None:
raise AirborneBootstrapError(
"airborne_bootstrap: cannot construct "
@@ -721,6 +725,185 @@ def _build_c3_lightglue_runtime(
return LightGlueRuntime(engine_handle)
def _load_camera_calibration(config: Config) -> CameraCalibration:
"""Read the camera calibration JSON into a :class:`CameraCalibration` DTO.
Mirrors the on-disk JSON shape that
:func:`gps_denied_onboard.runtime_root._replay_branch._load_camera_calibration`
already accepts (same calibration file the live and replay binaries
share). Replicated here \u2014 not imported from ``_replay_branch`` \u2014 because
the replay-branch helper raises ``CompositionError`` (replay-flow
contract) where the airborne bootstrap MUST raise
:class:`AirborneBootstrapError` per the AZ-618 umbrella's
operator-error contract. Both helpers consume the same on-disk
format; any future change to that format MUST land in lockstep
here and in ``_replay_branch.py``.
AZ-623 unit tests monkey-patch this function with a sentinel
:class:`CameraCalibration` so they exercise the
:func:`_build_c5_imu_preintegrator` wiring without an on-disk JSON
file (per the same Tier-2 monkeypatch pattern the AZ-622 builders
use for the heavy LightGlue seam).
"""
import numpy as np
path = config.runtime.camera_calibration_path
if not path:
raise AirborneBootstrapError(
"airborne_bootstrap: cannot construct "
"pre_constructed['c5_imu_preintegrator'] because "
"config.runtime.camera_calibration_path is empty. "
"Consuming component: c5_state. Production main() (AZ-624) "
"must populate the path to the camera calibration JSON; "
"tests stub _load_camera_calibration via monkeypatch."
)
calib_path = Path(path)
try:
blob = json.loads(calib_path.read_text(encoding="utf-8"))
except OSError as exc:
raise AirborneBootstrapError(
f"airborne_bootstrap: cannot construct "
f"pre_constructed['c5_imu_preintegrator'] because the camera "
f"calibration file at {path!r} could not be read: {exc!r}. "
f"Consuming component: c5_state. Ensure "
f"config.runtime.camera_calibration_path points at a readable "
f"JSON file."
) from exc
except json.JSONDecodeError as exc:
raise AirborneBootstrapError(
f"airborne_bootstrap: cannot construct "
f"pre_constructed['c5_imu_preintegrator'] because the camera "
f"calibration file at {path!r} is not valid JSON: {exc!r}. "
f"Consuming component: c5_state. Validate the calibration JSON "
f"shape against the on-disk format documented in "
f"runtime_root._replay_branch._load_camera_calibration."
) from exc
if not isinstance(blob, Mapping):
raise AirborneBootstrapError(
f"airborne_bootstrap: cannot construct "
f"pre_constructed['c5_imu_preintegrator'] because the camera "
f"calibration at {path!r} must decode to a JSON object; got "
f"{type(blob).__name__}. Consuming component: c5_state."
)
intrinsics = np.asarray(blob.get("intrinsics_3x3"), dtype=np.float64)
if intrinsics.shape != (3, 3):
raise AirborneBootstrapError(
f"airborne_bootstrap: cannot construct "
f"pre_constructed['c5_imu_preintegrator'] because the camera "
f"calibration at {path!r} 'intrinsics_3x3' must be 3x3; got "
f"shape {intrinsics.shape}. Consuming component: c5_state."
)
distortion = np.asarray(blob.get("distortion", []), dtype=np.float64)
body_to_camera = np.asarray(
blob.get("body_to_camera_se3", np.eye(4).tolist()),
dtype=np.float64,
)
return CameraCalibration(
camera_id=str(blob.get("camera_id", "airborne-camera")),
intrinsics_3x3=intrinsics,
distortion=distortion,
body_to_camera_se3=body_to_camera,
acquisition_method=str(blob.get("acquisition_method", "operator")),
metadata=dict(blob.get("metadata", {})),
)
def _build_c282_ransac_filter(config: Config) -> RansacFilter:
"""Build ``pre_constructed['c282_ransac_filter']`` for the airborne binary.
:class:`RansacFilter` is a static-only OpenCV wrapper (per AZ-282 /
E-CC-HELPERS); a fresh instance carries no state. Consumers
(``c3_matcher``, ``c3_5_adhop``, ``c4_pose``) dispatch to its static
methods, so identity-share is irrelevant \u2014 AC-623.2 explicitly
permits a fresh instance per :func:`build_pre_constructed` call.
No ``BUILD_*`` flag check applies: the helper is a CPU-only OpenCV
wrapper with no compile-time gate.
"""
del config # placeholder for future config-driven RANSAC variant selection
return RansacFilter()
def _build_c5_imu_preintegrator(config: Config) -> ImuPreintegrator:
"""Build (or retrieve cached) ``pre_constructed['c5_imu_preintegrator']``.
Reads ``config.runtime.camera_calibration_path``, loads the
:class:`CameraCalibration` DTO via :func:`_load_camera_calibration`,
and constructs an :class:`ImuPreintegrator` via
:func:`make_imu_preintegrator`. The preintegrator is cached at module
level keyed by the calibration path \u2014 AC-623.2 requires that two
invocations of :func:`build_pre_constructed` return the SAME
instance for the same path, so the bias / sample accumulator is not
silently reset across re-invocations.
Raises:
AirborneBootstrapError: when ``camera_calibration_path`` is
empty / unreadable / malformed (AC-623.3); message names
both the missing input AND the consuming component slug
``c5_state`` per the AZ-618 umbrella's operator-error
contract.
"""
path = config.runtime.camera_calibration_path
if not path:
raise AirborneBootstrapError(
"airborne_bootstrap: cannot construct "
"pre_constructed['c5_imu_preintegrator'] because "
"config.runtime.camera_calibration_path is empty. "
"Consuming component: c5_state. Production main() (AZ-624) "
"must populate the path before calling build_pre_constructed; "
"tests stub _load_camera_calibration via monkeypatch."
)
cached = _IMU_PREINTEGRATOR_CACHE.get(path)
if cached is not None:
return cached
calibration = _load_camera_calibration(config)
preintegrator = make_imu_preintegrator(calibration)
_IMU_PREINTEGRATOR_CACHE[path] = preintegrator
return preintegrator
def _build_c5_se3_utils(config: Config) -> Any:
"""Build ``pre_constructed['c5_se3_utils']`` for the airborne binary.
Returns the :mod:`gps_denied_onboard.helpers.se3_utils` module
itself as the namespace handle. Python modules support attribute
access for their public names (``exp_map``, ``log_map``,
``matrix_to_se3``, ``se3_to_matrix``, ``adjoint``,
``is_valid_rotation``, ``SE3``); both
:class:`OpenCVGtsamPoseEstimator` and
:class:`GtsamIsam2StateEstimator` store the injected handle as
``self._se3_utils: Any`` and dispatch via attribute access, so the
module satisfies the contract without an extra wrapper class. The
existing C5 unit-test fixtures (e.g.
``tests/unit/c5_state/test_az386_eskf_baseline.py``) inject
``mock.MagicMock()`` for the same slot \u2014 attribute-access shape
matches.
Returning the module also satisfies AC-623.2's caching note
incidentally: Python's import machinery returns the same module
object across calls, so two invocations of
:func:`build_pre_constructed` see the SAME ``c5_se3_utils`` value.
"""
del config # the module-as-namespace selection is config-independent
from gps_denied_onboard.helpers import se3_utils as se3_utils_module
return se3_utils_module
def _build_c5_wgs_converter(config: Config) -> WgsConverter:
"""Build ``pre_constructed['c5_wgs_converter']`` for the airborne binary.
:class:`WgsConverter` is a stateless static-only class (per AZ-279);
a fresh instance carries no module-level state beyond pyproj's
cached transformer pair. Returns ``WgsConverter()`` to match the
same construction pattern :mod:`runtime_root._replay_branch`
already uses (``wgs_converter = WgsConverter()`` at line 205). No
``BUILD_*`` flag check applies.
"""
del config # placeholder for future config-driven coord-system selection
return WgsConverter()
def _build_c3_feature_extractor(config: Config) -> FeatureExtractor:
"""Build ``pre_constructed['c3_feature_extractor']`` for the airborne binary.
@@ -756,15 +939,27 @@ def build_pre_constructed(config: Config) -> dict[str, Any]:
added the two C6 storage entries (``c6_descriptor_index`` +
``c6_tile_store``). AZ-621 (Phase C) added ``c7_inference``
(PyTorch FP16 vs. TensorRT, gated by
:data:`C7_AIRBORNE_BUILD_FLAGS`). AZ-622 (Phase D) adds
:data:`C7_AIRBORNE_BUILD_FLAGS`). AZ-622 (Phase D) added
``c3_lightglue_runtime`` (single shared
:class:`gps_denied_onboard.helpers.lightglue_runtime.LightGlueRuntime`
instance, gated by :data:`C3_MATCHER_BUILD_FLAGS` per the
configured strategy) + ``c3_feature_extractor`` (the shared
:class:`gps_denied_onboard.helpers.feature_extractor.FeatureExtractor`
used by C2.5). Phases E..F (AZ-623..AZ-624) will extend this
function to populate the remaining keys in
:data:`AIRBORNE_REQUIRED_PRE_CONSTRUCTED_KEYS`.
used by C2.5). AZ-623 (Phase E) adds the four stateless / cached c5
helpers: ``c282_ransac_filter`` (shared
:class:`gps_denied_onboard.helpers.ransac_filter.RansacFilter`),
``c5_imu_preintegrator`` (per-calibration-path-cached
:class:`gps_denied_onboard.helpers.imu_preintegrator.ImuPreintegrator`),
``c5_se3_utils`` (the
:mod:`gps_denied_onboard.helpers.se3_utils` module as a
namespace handle), and ``c5_wgs_converter`` (shared
:class:`gps_denied_onboard.helpers.wgs_converter.WgsConverter`).
The ``c5_isam2_graph_handle`` slot is the special-case ordering
work tracked separately in AZ-625 (split out of AZ-623 on
2026-05-19 because Path 1 of the AZ-623 spec required a
Protocol seam change forbidden by the AZ-618 umbrella). Phase F
(AZ-624) will wire main() and verify AC-1..AC-5 once both AZ-623
and AZ-625 land.
Returns a fresh dict on each call. The ``c13_fdr`` instance is cached
inside :func:`make_fdr_client` (per-producer cache) so two calls within
@@ -776,7 +971,13 @@ def build_pre_constructed(config: Config) -> dict[str, Any]:
caching at this layer; the C7 :class:`InferenceRuntime` built for the
``c7_inference`` slot is reused as the engine source for the LightGlue
matcher load (AZ-622) so the bootstrap does not double-build the
inference runtime.
inference runtime. AZ-623's ``c5_imu_preintegrator`` is cached at module
level (:data:`_IMU_PREINTEGRATOR_CACHE`) keyed by
``config.runtime.camera_calibration_path`` so its bias / sample
accumulator survives a re-invocation. The remaining AZ-623 c5 helpers
are stateless: ``c282_ransac_filter`` and ``c5_wgs_converter`` are
fresh static-only instances; ``c5_se3_utils`` is the
:mod:`gps_denied_onboard.helpers.se3_utils` module.
Replay-mode override: :func:`compose_root` merges ``replay_components``
over ``pre_constructed`` so the :class:`WallClock` here is replaced by
@@ -794,8 +995,11 @@ def build_pre_constructed(config: Config) -> dict[str, Any]:
requires ``c7_inference``; OR if the configured C3 matcher
strategy's :data:`C3_MATCHER_BUILD_FLAGS` flag is OFF (or
the strategy is unknown), or if the LightGlue engine load
fails. The message names the consuming component slug(s)
and the relevant gating flag(s).
fails; OR (AZ-623) if ``config.runtime.camera_calibration_path``
is empty / unreadable / malformed JSON, blocking the
``c5_imu_preintegrator`` build. The message names the
consuming component slug(s) and the relevant gating flag(s)
or missing inputs.
"""
constructed: dict[str, Any] = {}
constructed["c13_fdr"] = make_fdr_client(AIRBORNE_MAIN_PRODUCER_ID, config)
@@ -807,6 +1011,10 @@ def build_pre_constructed(config: Config) -> dict[str, Any]:
config, inference_runtime=constructed["c7_inference"]
)
constructed["c3_feature_extractor"] = _build_c3_feature_extractor(config)
constructed["c282_ransac_filter"] = _build_c282_ransac_filter(config)
constructed["c5_imu_preintegrator"] = _build_c5_imu_preintegrator(config)
constructed["c5_se3_utils"] = _build_c5_se3_utils(config)
constructed["c5_wgs_converter"] = _build_c5_wgs_converter(config)
return constructed
@@ -851,8 +1059,7 @@ def register_airborne_strategies() -> None:
"kv": {
"slots": [slug for slug, *_ in _AIRBORNE_REGISTRATIONS],
"total_registrations": sum(
len(strategies)
for _, strategies, *_ in _AIRBORNE_REGISTRATIONS
len(strategies) for _, strategies, *_ in _AIRBORNE_REGISTRATIONS
),
},
},