mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 10:31:13 +00:00
Oleksandr Bezdieniezhnykh
db27e25630
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>
70 lines
2.5 KiB
Python
70 lines
2.5 KiB
Python
"""C4 ``PoseEstimator`` Protocol (AZ-355).
|
|
|
|
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 TYPE_CHECKING, Protocol, runtime_checkable
|
|
|
|
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):
|
|
"""Single-pose estimator producing WGS84 + 6x6 covariance + provenance label.
|
|
|
|
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.
|
|
"""
|