[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
+21 -7
View File
@@ -25,13 +25,20 @@ from __future__ import annotations
import logging
from datetime import datetime, timezone
from unittest import mock
from uuid import UUID
import gtsam
import numpy as np
import pytest
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.nav import ImuSample, ImuWindow
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard._types.pose import (
CovarianceMode,
PoseEstimate,
PoseSourceLabel,
Quat,
)
from gps_denied_onboard._types.vio import VioOutput
from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.errors import EstimatorDegradedError
@@ -79,15 +86,22 @@ def _make_pose(
*,
frame_id: int,
t_seconds: float,
covariance_mode: str = "marginals",
pose: np.ndarray | None = None,
covariance_mode: CovarianceMode | str = CovarianceMode.MARGINALS,
) -> PoseEstimate:
mode = (
covariance_mode
if isinstance(covariance_mode, CovarianceMode)
else CovarianceMode(covariance_mode.lower())
)
return PoseEstimate(
frame_id=frame_id,
timestamp=datetime.fromtimestamp(t_seconds, tz=timezone.utc),
pose_se3=pose if pose is not None else np.eye(4),
frame_id=UUID(int=frame_id),
position_wgs84=LatLonAlt(lat_deg=0.0, lon_deg=0.0, alt_m=0.0),
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
covariance_6x6=np.eye(6) * 0.01,
covariance_mode=covariance_mode,
covariance_mode=mode,
source_label=PoseSourceLabel.SATELLITE_ANCHORED,
last_satellite_anchor_age_ms=0,
emitted_at=int(t_seconds * 1_000_000_000),
)