mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 20:21: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:
@@ -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