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