mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 18:01:13 +00:00
[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:
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user