[AZ-355] C4 PoseEstimator Protocol + factory + DTOs + composition

Land the foundational C4 surface AZ-358 (Marginals) and AZ-361
(Hybrid) build on top of:

- PoseEstimator Protocol (@runtime_checkable): estimate(...) +
  current_covariance_mode().
- Error hierarchy: PoseEstimatorError, PnpFailureError,
  PoseEstimatorConfigError; CovarianceDegradedWarning as a Warning
  subclass (warnings.warn path, not raised).
- ISam2GraphHandle Protocol stub (READ-ONLY view, get_pose_key only)
  decoupled from C5's concrete ISam2GraphHandleImpl.
- C4PoseConfig (frozen dataclass) + register on c4_pose import.
- runtime_root/pose_factory.build_pose_estimator with lazy-import
  fallback; INFO log c4.pose.strategy_loaded; shares ingest-thread
  binding with C5 per ADR-003.

DTO restructuring (cross-cutting): retire the legacy raw-4x4
PoseEstimate(int frame_id, datetime timestamp, pose_se3, ...) and
ship the contract shape PoseEstimate(UUID, LatLonAlt, Quat,
np.ndarray, CovarianceMode, PoseSourceLabel,
last_satellite_anchor_age_ms, emitted_at). C5 add_pose_anchor in
both gtsam_isam2 + eskf_baseline migrated in lockstep via
WGS84->ENU + Quat->R helpers; test fixtures updated. VIO output
stays on the raw shape until AZ-331 (C1 protocol) lands.

LatLonAlt upgraded to slots=True per AC-2. ThermalState stub added
to _types/thermal.py so the Protocol typechecks pre-AZ-302.

Tests: 25 new in tests/unit/c4_pose/test_az355_pose_protocol.py
covering AC-1..AC-10 + factory wiring + config validation; full
repo: 685 passed, 2 pre-existing CI-only skips.

Jira transition deferred: MCP "Not connected"; leftover entry in
_docs/_process_leftovers/2026-05-11_jira_transition_az355_deferred.md.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 10:32:14 +03:00
parent c0bdb57957
commit db27e25630
19 changed files with 1407 additions and 52 deletions
+7 -2
View File
@@ -9,9 +9,14 @@ from __future__ import annotations
from dataclasses import dataclass
@dataclass(frozen=True)
@dataclass(frozen=True, slots=True)
class LatLonAlt:
"""A WGS84 geographic position. ``alt_m`` is height above the WGS84 ellipsoid."""
"""A WGS84 geographic position. ``alt_m`` is height above the WGS84 ellipsoid.
``slots=True`` per AZ-355 AC-2 — DTOs cross component boundaries
and mutation-through-aliasing has bitten this codebase before
(R05 in the C5 risk register, R10 in the C4 risk register).
"""
lat_deg: float
lon_deg: float
+85 -18
View File
@@ -1,28 +1,95 @@
"""C4 PoseEstimator output DTOs.
"""C4 PoseEstimator output DTOs + enums (AZ-355 / E-C4).
The C5 estimator output + provenance enums live in
:mod:`gps_denied_onboard._types.state`; importing them here used to be
convenient for the C4 module's public re-exports but the two DTOs
diverged (C5 carries a UUID frame_id + WGS84 position + Quat
orientation directly; C4 still passes a raw 4x4 ``pose_se3`` to keep
the OpenCV ↔ GTSAM seam thin). Components that need the C5 surface
import from ``_types.state`` directly.
Aligned with the C4 contract at
``_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md``
v1.0.0. The structured DTO (``UUID frame_id`` + ``LatLonAlt`` +
``Quat`` + ``np.ndarray covariance_6x6`` + enums + ``emitted_at``
monotonic_ns) is consumed by C5 (state estimator), C8 (FC outbound),
and C13 (FDR records). The C4 contract makes this the canonical form;
no component should re-invent the raw-4x4 ``pose_se3`` shape that
predated AZ-355.
``Quat`` is re-exported from :mod:`_types.state` so the C5 surface
(``EstimatorOutput.orientation_world_T_body``) and the C4 surface
(``PoseEstimate.orientation_world_T_body``) share one literal class
identity — ``isinstance`` checks across the boundary stay free.
``LatLonAlt`` is re-exported from :mod:`_types.geo` for the same
reason; the geo module remains the storage site so downstream
non-C4 callers (``WgsConverter`` + C6 tile-cache + C8 outbound)
don't pull in the C4 namespace.
``PoseSourceLabel`` is shared with C5 (one provenance enum, three
values: ``SATELLITE_ANCHORED`` / ``VISUAL_PROPAGATED`` /
``DEAD_RECKONED``).
"""
from __future__ import annotations
from dataclasses import dataclass
from datetime import datetime
from typing import Any
from enum import Enum
from typing import TYPE_CHECKING, Any
from uuid import UUID
if TYPE_CHECKING:
import numpy.typing as npt
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.state import PoseSourceLabel, Quat
__all__ = [
"CovarianceMode",
"LatLonAlt",
"PoseEstimate",
"PoseSourceLabel",
"Quat",
]
@dataclass(frozen=True)
class CovarianceMode(Enum):
"""Per-frame covariance-derivation mode tagged on every emitted ``PoseEstimate``.
* ``MARGINALS`` — production-default; recovered via
``Marginals.marginalCovariance(pose_key)`` against C5's shared
iSAM2 graph (ADR-003 substrate). The cost-accurate path.
* ``JACOBIAN`` — D-CROSS-LATENCY-1 thermal-throttle fallback
(ADR-006). Cheaper but ~5-10% less accurate; engaged per-frame
when ``ThermalState.throttle == True``.
AZ-355 owns the enum; AZ-358 (Marginals) + AZ-361 (Hybrid) own
the producer paths.
"""
MARGINALS = "marginals"
JACOBIAN = "jacobian"
@dataclass(frozen=True, slots=True)
class PoseEstimate:
"""A single 6-DoF pose estimate with covariance."""
"""C4 single-frame pose estimate.
frame_id: int
timestamp: datetime
pose_se3: Any
covariance_6x6: Any | None = None
covariance_mode: str = "marginals"
mre_px: float | None = None
Per the C4 contract (Invariant 5) ``covariance_6x6`` is always
SPD. Per Invariant 6 the ``covariance_mode`` enum matches the
path actually taken — never reports ``MARGINALS`` while the
implementation ran the Jacobian path. Per Invariant 7 C4 emits
``source_label = SATELLITE_ANCHORED`` unconditionally on success;
only C5 may downgrade to ``VISUAL_PROPAGATED`` or
``DEAD_RECKONED``.
``last_satellite_anchor_age_ms`` is provided BY C5 and passed
through (Invariant 8) — the runtime root broadcasts the current
value from C5; C4 caches it without computing it independently.
``emitted_at`` is a ``time.monotonic_ns()`` snapshot taken when
the estimator produced the value; consumers MUST NOT compare it
to wall-clock ``datetime`` values.
"""
frame_id: UUID
position_wgs84: LatLonAlt
orientation_world_T_body: Quat
covariance_6x6: npt.NDArray[Any]
covariance_mode: CovarianceMode
source_label: PoseSourceLabel
last_satellite_anchor_age_ms: int
emitted_at: int
+32
View File
@@ -0,0 +1,32 @@
"""C7 ``ThermalState`` DTO stub (forward-declared for AZ-355).
AZ-355 (C4 PoseEstimator Protocol) needs a ``ThermalState`` type to
annotate :meth:`PoseEstimator.estimate`. The full producer
(``ThermalStatePublisher`` in C7) is owned by AZ-302; this module
holds the minimal DTO surface C4 needs so the Protocol typechecks
without a circular dependency or a ``TYPE_CHECKING`` workaround.
When AZ-302 lands, it MAY add fields here (temperature reading,
thermal-zone source, captured_at) but MUST keep the ``throttle``
boolean — it is the only field the C4 Protocol contract pins.
"""
from __future__ import annotations
from dataclasses import dataclass
__all__ = ["ThermalState"]
@dataclass(frozen=True, slots=True)
class ThermalState:
"""C7-reported thermal state consumed by C4 for the per-frame mode switch.
``throttle == True`` triggers the Jacobian path (D-CROSS-LATENCY-1
/ ADR-006). ``False`` keeps the production Marginals path.
The full C7 publisher (AZ-302) emits these on a fixed cadence;
C4 reads the latest value at every ``estimate`` call entry.
"""
throttle: bool
@@ -1,6 +1,52 @@
"""C4 Pose Estimator component — Public API."""
"""C4 PoseEstimator component — public API (AZ-355 / E-C4).
from gps_denied_onboard._types.pose import PoseEstimate
Per the C4 contract (``state_estimator_protocol.md`` v1.0.0), the
public surface consists of:
* :class:`PoseEstimator` Protocol
* :class:`PoseEstimate` DTO + :class:`LatLonAlt` + :class:`Quat`
(re-exported from ``_types.pose``)
* :class:`CovarianceMode` + :class:`PoseSourceLabel` enums
* Error hierarchy: :class:`PoseEstimatorError` + subclasses +
:class:`CovarianceDegradedWarning`
* :class:`C4PoseConfig` config block (registered on import)
The ``ISam2GraphHandle`` Protocol stub lives in the private
``_isam2_handle`` module — the composition root imports it directly;
it is intentionally NOT part of the public surface to discourage
non-runtime-root callers from poking at C5's internals.
"""
from gps_denied_onboard._types.pose import (
CovarianceMode,
LatLonAlt,
PoseEstimate,
PoseSourceLabel,
Quat,
)
from gps_denied_onboard.components.c4_pose.config import C4PoseConfig
from gps_denied_onboard.components.c4_pose.errors import (
CovarianceDegradedWarning,
PnpFailureError,
PoseEstimatorConfigError,
PoseEstimatorError,
)
from gps_denied_onboard.components.c4_pose.interface import PoseEstimator
from gps_denied_onboard.config.schema import register_component_block
__all__ = ["PoseEstimate", "PoseEstimator"]
__all__ = [
"C4PoseConfig",
"CovarianceDegradedWarning",
"CovarianceMode",
"LatLonAlt",
"PnpFailureError",
"PoseEstimate",
"PoseEstimator",
"PoseEstimatorConfigError",
"PoseEstimatorError",
"PoseSourceLabel",
"Quat",
]
register_component_block("c4_pose", C4PoseConfig)
@@ -0,0 +1,37 @@
"""C4 ``ISam2GraphHandle`` Protocol stub (AZ-355).
The C5 iSAM2 graph is the shared GTSAM substrate (ADR-003). C4
NEVER owns the graph — it receives a typed handle from the runtime
root and uses ONLY the minimum surface it needs to attach pose
factors against C5's key namespace.
Per the C4 contract this Protocol exposes ONLY
``get_pose_key(frame_id) -> int`` — the C5 concrete handle
(``components.c5_state._isam2_handle.ISam2GraphHandleImpl``) is
strictly a superset of this surface, so a duck-typed satisfaction
check via ``isinstance(handle, ISam2GraphHandle)`` succeeds without
C4 importing C5 internals.
Risk-1 mitigation per the AZ-355 task spec: if C5's graph design
grows, this stub grows ONLY if C4's needs grow. Otherwise the two
Protocols diverge cleanly along the producer/consumer split.
"""
from __future__ import annotations
from typing import Protocol, runtime_checkable
__all__ = ["ISam2GraphHandle"]
@runtime_checkable
class ISam2GraphHandle(Protocol):
"""Read-only view of C5's iSAM2 graph for C4's pose-factor adds."""
def get_pose_key(self, frame_id: int) -> int:
"""Map a C4 frame_id to the corresponding GTSAM pose key.
C5 owns the key allocation policy (typically
``gtsam.symbol('x', frame_id)``); C4 only needs the integer
key to construct a ``PriorFactorPose3``.
"""
@@ -0,0 +1,64 @@
"""C4 pose-estimator config block (AZ-355).
Registered into the global config registry via
``register_component_block("c4_pose", C4PoseConfig)`` on import of
``gps_denied_onboard.components.c4_pose``. The runtime root reads
``config.components["c4_pose"]`` and dispatches to the
``opencv_gtsam`` strategy (the only one defined; the Protocol exists
for ADR-009).
"""
from __future__ import annotations
from dataclasses import dataclass
from typing import Final
from gps_denied_onboard.config.schema import ConfigError
__all__ = ["KNOWN_POSE_STRATEGIES", "C4PoseConfig"]
KNOWN_POSE_STRATEGIES: Final[frozenset[str]] = frozenset({"opencv_gtsam"})
@dataclass(frozen=True)
class C4PoseConfig:
"""C4 pose-estimator config block.
Fields per the C4 contract §"Config-load-time validation":
* ``strategy`` — selects the concrete estimator. Currently only
``"opencv_gtsam"`` is defined.
* ``ransac_iterations`` — OpenCV ``solvePnPRansac`` iteration
budget. Default 200 per the contract.
* ``ransac_reprojection_threshold_px`` — RANSAC inlier-distance
threshold. Default 4.0 pixels per the contract.
* ``thermal_throttle_threshold_celsius`` — informational only;
the actual ``ThermalState.throttle`` decision is owned by C7
(AZ-302). Default 75.0 °C.
"""
strategy: str = "opencv_gtsam"
ransac_iterations: int = 200
ransac_reprojection_threshold_px: float = 4.0
thermal_throttle_threshold_celsius: float = 75.0
def __post_init__(self) -> None:
if self.strategy not in KNOWN_POSE_STRATEGIES:
raise ConfigError(
f"C4PoseConfig.strategy={self.strategy!r} not in {sorted(KNOWN_POSE_STRATEGIES)}"
)
if self.ransac_iterations <= 0:
raise ConfigError(
f"C4PoseConfig.ransac_iterations must be > 0; got {self.ransac_iterations}"
)
if self.ransac_reprojection_threshold_px <= 0.0:
raise ConfigError(
"C4PoseConfig.ransac_reprojection_threshold_px must be > 0; "
f"got {self.ransac_reprojection_threshold_px}"
)
if self.thermal_throttle_threshold_celsius <= 0.0:
raise ConfigError(
"C4PoseConfig.thermal_throttle_threshold_celsius must be > 0; "
f"got {self.thermal_throttle_threshold_celsius}"
)
@@ -0,0 +1,59 @@
"""C4 PoseEstimator error hierarchy (AZ-355).
Per the C4 contract (Invariant 9) and AC-4: ``PnpFailureError`` is
the ONLY non-warning exception that escapes ``PoseEstimator.estimate``.
``CovarianceDegradedWarning`` is a Python ``Warning`` (NOT an
``Exception``) — emitted via ``warnings.warn(...)`` at the start of
every Jacobian-path frame so a ``try / except Exception`` block in
the caller does NOT swallow it. R10 acceptance for the C4 contract
hangs on this distinction.
``PoseEstimatorConfigError`` is raised by the composition-root
factory (``runtime_root.pose_factory.build_pose_estimator``) for
unknown strategies, missing dependencies, or thread-binding
violations.
"""
from __future__ import annotations
__all__ = [
"CovarianceDegradedWarning",
"PnpFailureError",
"PoseEstimatorConfigError",
"PoseEstimatorError",
]
class PoseEstimatorError(Exception):
"""Base class for all C4 ``PoseEstimator`` errors."""
class PnpFailureError(PoseEstimatorError):
"""RANSAC convergence failure or degenerate match geometry.
Per Invariant 9 this is NEVER converted to a fallback
``PoseEstimate`` inside C4 — the fallback decision belongs to
C5 (AZ-385's source-label gate + AZ-388's no-estimate watchdog).
"""
class PoseEstimatorConfigError(PoseEstimatorError):
"""Composition-root factory rejection.
Raised by ``build_pose_estimator`` for unknown ``config.pose.strategy``
values, missing constructor dependencies, or attempted thread-binding
violations.
"""
class CovarianceDegradedWarning(Warning):
"""Per-frame thermal-state-driven Jacobian-path engagement.
NOT an exception. Emitted via ``warnings.warn(...)`` by the
Jacobian path (AZ-361 / D-CROSS-LATENCY-1). Per AC-4 callers MUST
use ``warnings.catch_warnings(...)`` or
``warnings.simplefilter("once", CovarianceDegradedWarning)`` if
they need to observe these warnings — a ``try / except Exception``
block does NOT catch them, because ``Warning`` does not subclass
``Exception`` in Python.
"""
@@ -1,20 +1,69 @@
"""C4 `PoseEstimator` Protocol.
"""C4 ``PoseEstimator`` Protocol (AZ-355).
Concrete impl: OpenCV `solvePnPRansac` + GTSAM Marginals. See
`_docs/02_document/components/06_c4_pose/`.
Per ADR-009 (interface-first DI) consumers (C5, runtime root, tests)
hold a typed reference to ``PoseEstimator`` rather than the concrete
``OpenCVGtsamPoseEstimator`` (AZ-358) class. ADR-002 build-time
exclusion does NOT apply — exactly one concrete implementation
exists — but the Protocol + factory wiring matches the C2 / C2.5 /
C3 / C3.5 pattern for symmetry.
The Protocol surface is two methods:
* :meth:`PoseEstimator.estimate` — full per-frame pose estimation
pipeline (PnP + factor add + covariance recovery). Raises
:class:`PnpFailureError` on RANSAC failure.
* :meth:`PoseEstimator.current_covariance_mode` — exposes the
per-frame decision (marginals / jacobian) for C5 FDR provenance
and the C4-IT-03 mode-switch test.
The Protocol is ``@runtime_checkable`` so test fakes pass
``isinstance(fake, PoseEstimator)``.
"""
from __future__ import annotations
from typing import Protocol
from typing import TYPE_CHECKING, Protocol, runtime_checkable
from gps_denied_onboard._types.matching import MatchResult
from gps_denied_onboard._types.pose import PoseEstimate
if TYPE_CHECKING:
from gps_denied_onboard._types.calibration import CameraCalibration
from gps_denied_onboard._types.matching import MatchResult
from gps_denied_onboard._types.pose import CovarianceMode, PoseEstimate
from gps_denied_onboard._types.thermal import ThermalState
__all__ = ["PoseEstimator"]
@runtime_checkable
class PoseEstimator(Protocol):
"""Estimate a 6-DoF pose from a verified cross-domain match."""
"""Single-pose estimator producing WGS84 + 6x6 covariance + provenance label.
def estimate(self, match: MatchResult) -> PoseEstimate: ...
Stateless per-frame except for the constructor-injected shared
GTSAM substrate (owned by C5). Per Invariant 1 the implementation
is bound to the same ingest thread as C5 (composition root
enforces).
"""
def estimate(
self,
match_result: MatchResult,
calibration: CameraCalibration,
thermal_state: ThermalState,
) -> PoseEstimate:
"""Run PnP → factor add → covariance recovery.
Per-frame thermal decision: ``thermal_state.throttle == True``
engages the Jacobian path (cheap, ~5-10 % accuracy loss);
``False`` engages the Marginals path (production default).
Raises:
PnpFailureError: RANSAC convergence failure or degenerate
match geometry. C5 owns the fallback decision; this
method NEVER returns a fallback ``PoseEstimate``.
"""
def current_covariance_mode(self) -> CovarianceMode:
"""Return the mode used for the LAST :meth:`estimate` call.
Consumed by C5 for FDR provenance and by C4-IT-03 to verify
the per-frame mode switch.
"""
@@ -372,9 +372,9 @@ class EskfStateEstimator(StateEstimator):
has no graph to throttle); it integrates every anchor as a
regular measurement.
"""
ts_ns = _datetime_to_ns(pose.timestamp)
ts_ns = int(pose.emitted_at)
self._guard_timestamp(ts_ns, source="pose_anchor")
meas_pose = _pose_se3_to_array(pose.pose_se3)
meas_pose = self._pose_estimate_to_matrix(pose)
# Both modes are treated identically by the ESKF — the
# JACOBIAN exclusion is iSAM2-graph-specific. AC-4.
@@ -641,6 +641,29 @@ class EskfStateEstimator(StateEstimator):
)
return WgsConverter.local_enu_to_latlonalt(origin, enu)
def _pose_estimate_to_matrix(self, pose: PoseEstimate) -> np.ndarray:
"""Convert a C4 :class:`PoseEstimate` to a 4x4 homogeneous matrix.
WGS84 → ENU via the injected origin (matches the iSAM2
estimator's helper); ``Quat`` → 3x3 rotation via the local
``_quat_to_rot`` helper applied to a freshly-built quaternion.
"""
origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN
try:
enu = WgsConverter.latlonalt_to_local_enu(origin, pose.position_wgs84)
except Exception as exc:
raise EstimatorDegradedError(
f"eskf add_pose_anchor: WGS84→ENU failed for frame {pose.frame_id}: {exc}"
) from exc
q = pose.orientation_world_T_body
rotation = _quat_to_rot(
np.array([float(q.w), float(q.x), float(q.y), float(q.z)], dtype=np.float64)
)
matrix = np.eye(4, dtype=np.float64)
matrix[:3, :3] = rotation
matrix[:3, 3] = enu
return matrix
def _compute_last_anchor_age_ms(self, now_ns: int) -> int:
if self._last_anchor_ns == 0:
return now_ns // 1_000_000
@@ -476,11 +476,11 @@ class GtsamIsam2StateEstimator(StateEstimator):
gate (AZ-385) and ``last_anchor_age_ms`` see a recent anchor.
"""
handle = self._require_handle()
ts_ns = _datetime_to_ns(pose.timestamp)
ts_ns = int(pose.emitted_at)
self._guard_timestamp(ts_ns, source="pose_anchor")
pose_key = self.key_for_frame(pose.frame_id)
mode = (pose.covariance_mode or "marginals").lower()
mode = pose.covariance_mode.value
# Both paths update the anchor freshness sentinel. The C5
# contract documents this — even the throttled JACOBIAN path
@@ -488,7 +488,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
self._last_anchor_ns = time.monotonic_ns()
if mode == "marginals":
gtsam_pose = _pose_se3_to_gtsam(pose.pose_se3)
gtsam_pose = _pose_se3_to_gtsam(self._pose_estimate_to_matrix(pose))
noise = _build_pose_noise(pose.covariance_6x6)
factor = gtsam.PriorFactorPose3(pose_key, gtsam_pose, noise)
try:
@@ -1063,6 +1063,26 @@ class GtsamIsam2StateEstimator(StateEstimator):
except Exception as exc:
raise EstimatorFatalError(f"_enu_pose_to_wgs84 failed: {exc}") from exc
def _pose_estimate_to_matrix(self, pose: PoseEstimate) -> np.ndarray:
"""Convert a C4 :class:`PoseEstimate` to a 4x4 homogeneous-transform matrix.
WGS84 → ENU via the injected origin; ``Quat`` → 3x3 rotation
via :func:`_quat_dto_to_rot`. The result is what GTSAM's
``Pose3`` constructor consumes.
"""
origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN
try:
enu = WgsConverter.latlonalt_to_local_enu(origin, pose.position_wgs84)
except Exception as exc:
raise EstimatorDegradedError(
f"add_pose_anchor: WGS84→ENU failed for frame {pose.frame_id}: {exc}"
) from exc
rotation = _quat_dto_to_rot(pose.orientation_world_T_body)
matrix = np.eye(4, dtype=np.float64)
matrix[:3, :3] = rotation
matrix[:3, 3] = enu
return matrix
def _latest_velocity_or_zero(self) -> tuple[float, float, float]:
"""Read the most-recent IMU keyframe's velocity or fall back to zero.
@@ -1258,6 +1278,19 @@ def _pose_se3_to_gtsam(pose_se3: Any) -> gtsam.Pose3:
return gtsam.Pose3(arr)
def _quat_dto_to_rot(q: Quat) -> np.ndarray:
"""Convert a :class:`Quat` DTO (scalar-first, body→world) to a 3x3 rotation."""
w, x, y, z = float(q.w), float(q.x), float(q.y), float(q.z)
return np.array(
[
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
],
dtype=np.float64,
)
def _build_pose_noise(covariance: Any | None) -> gtsam.noiseModel.Base:
"""Build a 6-DoF Gaussian noise model from a 6x6 covariance.
@@ -36,6 +36,12 @@ from gps_denied_onboard.runtime_root.fc_factory import (
register_fc_adapter,
register_gcs_adapter,
)
from gps_denied_onboard.runtime_root.pose_factory import (
build_pose_estimator,
clear_pose_registry,
list_registered_pose_strategies,
register_pose_estimator,
)
from gps_denied_onboard.runtime_root.spoof_recovery_sink import (
SpoofRecoveryPublisher,
SpoofRecoverySink,
@@ -72,8 +78,10 @@ __all__ = [
"bind_state_ingest_thread",
"build_fc_adapter",
"build_gcs_adapter",
"build_pose_estimator",
"build_state_estimator",
"clear_outbound_thread_binding",
"clear_pose_registry",
"clear_state_ingest_binding",
"clear_state_registry",
"clear_strategy_registries",
@@ -83,11 +91,13 @@ __all__ = [
"compose_root",
"list_registered_fc_strategies",
"list_registered_gcs_strategies",
"list_registered_pose_strategies",
"list_registered_state_strategies",
"list_registered_strategies",
"main",
"register_fc_adapter",
"register_gcs_adapter",
"register_pose_estimator",
"register_state_estimator",
"register_strategy",
"take_off",
@@ -0,0 +1,181 @@
"""Composition-root factory for C4 pose estimator (AZ-355 / E-C4).
Mirrors :mod:`gps_denied_onboard.runtime_root.state_factory` — per
ADR-001 the composition root is the single registration site for
all strategy factories. C4 has exactly ONE concrete strategy
(``opencv_gtsam`` → AZ-358); the Protocol exists for ADR-009
(interface-first DI) so callers don't import the concrete class.
The runtime root constructs the dependencies (RansacFilter,
WgsConverter, SE3Utils, ISam2GraphHandle) ONCE and passes references
through this factory. The factory does NOT instantiate them.
Per the C4 contract Invariant 1 + AC-9: the C4 estimator is bound
to the SAME ingest thread as C5 (ADR-003 shared GTSAM substrate is
not thread-safe). The thread-binding helper is shared with the C5
state factory (``bind_state_ingest_thread``); a second binding
from a different thread raises
:class:`StateIngestThreadAlreadyBoundError`.
"""
from __future__ import annotations
import importlib
from collections.abc import Callable
from typing import TYPE_CHECKING, Any, Final
from gps_denied_onboard.components.c4_pose._isam2_handle import ISam2GraphHandle
from gps_denied_onboard.components.c4_pose.config import (
KNOWN_POSE_STRATEGIES,
C4PoseConfig,
)
from gps_denied_onboard.components.c4_pose.errors import PoseEstimatorConfigError
from gps_denied_onboard.components.c4_pose.interface import PoseEstimator
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard.config import Config
__all__ = [
"PoseEstimatorFactory",
"build_pose_estimator",
"clear_pose_registry",
"list_registered_pose_strategies",
"register_pose_estimator",
]
PoseEstimatorFactory = Callable[..., PoseEstimator]
_POSE_REGISTRY: dict[str, PoseEstimatorFactory] = {}
# Single concrete strategy (ADR-001); module path for lazy-import
# fallback when the test/binary did not pre-register.
_STRATEGY_MODULE_PATHS: Final[dict[str, str]] = {
"opencv_gtsam": "gps_denied_onboard.components.c4_pose.opencv_gtsam_estimator",
}
def register_pose_estimator(strategy: str, factory: PoseEstimatorFactory) -> None:
"""Register a concrete ``PoseEstimator`` strategy.
Duplicate registration with a different factory raises
:class:`PoseEstimatorConfigError`.
"""
existing = _POSE_REGISTRY.get(strategy)
if existing is not None and existing is not factory:
raise PoseEstimatorConfigError(
f"duplicate PoseEstimator registration for strategy {strategy!r}"
)
_POSE_REGISTRY[strategy] = factory
def clear_pose_registry() -> None:
"""Reset the pose registry; unit-test isolation only."""
_POSE_REGISTRY.clear()
def list_registered_pose_strategies() -> list[str]:
return sorted(_POSE_REGISTRY)
def build_pose_estimator(
config: Config,
*,
ransac_filter: Any,
wgs_converter: Any,
se3_utils: Any,
isam2_graph_handle: ISam2GraphHandle,
) -> PoseEstimator:
"""Resolve + build the configured C4 pose estimator.
Validation order: config block lookup → strategy known? →
isam2_graph_handle conforms? → factory lookup (with lazy-import
fallback) → INFO log on success.
Raises:
PoseEstimatorConfigError: invalid config, unknown strategy,
non-conforming graph handle, or registry miss after
lazy-import fallback.
"""
block = _read_pose_block(config)
strategy = block.strategy
log = get_logger("runtime_root.pose_factory")
if strategy not in KNOWN_POSE_STRATEGIES:
log.error(
"c4.pose.unknown_strategy",
extra={
"kind": "c4.pose.unknown_strategy",
"kv": {
"strategy": strategy,
"known": sorted(KNOWN_POSE_STRATEGIES),
},
},
)
raise PoseEstimatorConfigError(
f"C4PoseConfig.strategy={strategy!r} not in {sorted(KNOWN_POSE_STRATEGIES)}"
)
if not isinstance(isam2_graph_handle, ISam2GraphHandle):
raise PoseEstimatorConfigError(
"build_pose_estimator: isam2_graph_handle does not satisfy "
"the C4 ISam2GraphHandle Protocol (missing get_pose_key?)"
)
factory = _resolve_factory(strategy)
estimator = factory(
config=config,
ransac_filter=ransac_filter,
wgs_converter=wgs_converter,
se3_utils=se3_utils,
isam2_graph_handle=isam2_graph_handle,
)
log.info(
f"c4.pose.strategy_loaded: strategy={strategy} "
f"ransac_iterations={block.ransac_iterations} "
f"ransac_reprojection_threshold_px={block.ransac_reprojection_threshold_px}",
extra={
"kind": "c4.pose.strategy_loaded",
"kv": {
"strategy": strategy,
"ransac_iterations": block.ransac_iterations,
"ransac_reprojection_threshold_px": block.ransac_reprojection_threshold_px,
"thermal_throttle_threshold_celsius": (block.thermal_throttle_threshold_celsius),
},
},
)
return estimator
def _read_pose_block(config: Config) -> C4PoseConfig:
components = getattr(config, "components", None) or {}
block = components.get("c4_pose") if isinstance(components, dict) else None
if block is None:
return C4PoseConfig()
if isinstance(block, C4PoseConfig):
return block
raise PoseEstimatorConfigError(
f"config.components['c4_pose'] must be a C4PoseConfig; got {type(block).__name__}"
)
def _resolve_factory(strategy: str) -> PoseEstimatorFactory:
factory = _POSE_REGISTRY.get(strategy)
if factory is not None:
return factory
module_path = _STRATEGY_MODULE_PATHS.get(strategy)
if module_path is None:
raise PoseEstimatorConfigError(
f"pose strategy {strategy!r} has no module-path mapping for lazy import"
)
try:
module = importlib.import_module(module_path)
except ImportError as exc:
raise PoseEstimatorConfigError(
f"pose strategy {strategy!r} module {module_path!r} not importable: {exc}"
) from exc
factory_obj = getattr(module, "create", None)
if factory_obj is None or not callable(factory_obj):
raise PoseEstimatorConfigError(
f"pose strategy {strategy!r} module {module_path!r} has no create(...) factory"
)
return factory_obj