[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>

tests/unit/c5_state/test_az383_factor_adds.py

@@ -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),
     )
 
 

tests/unit/c5_state/test_az386_eskf_baseline.py

@@ -29,14 +29,15 @@ import dataclasses
 from datetime import datetime, timezone
 from typing import Any
 from unittest import mock
-from uuid import uuid4
+from uuid import UUID, uuid4
 
 import numpy as np
 import pytest
 
 from gps_denied_onboard._types.fc import GpsHealth, GpsStatus
+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, Quat
 from gps_denied_onboard._types.state import IsamState, PoseSourceLabel
 from gps_denied_onboard._types.vio import VioOutput
 from gps_denied_onboard.components.c5_state import (
@@ -52,6 +53,7 @@ from gps_denied_onboard.components.c5_state.eskf_baseline import (
 )
 from gps_denied_onboard.config import load_config
 from gps_denied_onboard.config.schema import Config
+from gps_denied_onboard.helpers.wgs_converter import WgsConverter
 from gps_denied_onboard.runtime_root.state_factory import (
     build_state_estimator,
     clear_state_ingest_binding,
@@ -117,20 +119,60 @@ def _vio(
     )
 
 
+_ENU_ORIGIN = LatLonAlt(lat_deg=0.0, lon_deg=0.0, alt_m=0.0)
+
+
+def _rot_to_quat(R: np.ndarray) -> Quat:
+    """3x3 rotation → unit quaternion (w,x,y,z); standard robust formula."""
+    trace = R[0, 0] + R[1, 1] + R[2, 2]
+    if trace > 0:
+        s = 0.5 / np.sqrt(trace + 1.0)
+        w = 0.25 / s
+        x = (R[2, 1] - R[1, 2]) * s
+        y = (R[0, 2] - R[2, 0]) * s
+        z = (R[1, 0] - R[0, 1]) * s
+    elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
+        s = 2.0 * np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])
+        w = (R[2, 1] - R[1, 2]) / s
+        x = 0.25 * s
+        y = (R[0, 1] + R[1, 0]) / s
+        z = (R[0, 2] + R[2, 0]) / s
+    elif R[1, 1] > R[2, 2]:
+        s = 2.0 * np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])
+        w = (R[0, 2] - R[2, 0]) / s
+        x = (R[0, 1] + R[1, 0]) / s
+        y = 0.25 * s
+        z = (R[1, 2] + R[2, 1]) / s
+    else:
+        s = 2.0 * np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])
+        w = (R[1, 0] - R[0, 1]) / s
+        x = (R[0, 2] + R[2, 0]) / s
+        y = (R[1, 2] + R[2, 1]) / s
+        z = 0.25 * s
+    return Quat(w=float(w), x=float(x), y=float(y), z=float(z))
+
+
 def _pose_anchor(
     frame_id: int,
     ts_ns: int,
     pose: np.ndarray,
     cov: np.ndarray | None = None,
-    mode: str = "marginals",
+    mode: CovarianceMode | str = CovarianceMode.MARGINALS,
 ) -> PoseEstimate:
+    rotation = pose[:3, :3]
+    translation = np.ascontiguousarray(pose[:3, 3], dtype=np.float64)
+    position = WgsConverter.local_enu_to_latlonalt(_ENU_ORIGIN, translation)
+    quat = _rot_to_quat(rotation)
+    mode_enum = mode if isinstance(mode, CovarianceMode) else CovarianceMode(mode.lower())
     return PoseEstimate(
-        frame_id=frame_id,
-        timestamp=datetime.fromtimestamp(ts_ns / 1_000_000_000, tz=timezone.utc),
-        pose_se3=pose,
+        frame_id=UUID(int=frame_id),
+        position_wgs84=position,
+        orientation_world_T_body=quat,
         covariance_6x6=cov if cov is not None else np.eye(6) * 0.01,
-        covariance_mode=mode,
-        mre_px=0.5,
+        covariance_mode=mode_enum,
+        source_label=PoseSourceLabel.SATELLITE_ANCHORED,
+        last_satellite_anchor_age_ms=0,
+        emitted_at=ts_ns,
     )