[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
@@ -372,9 +372,9 @@ class EskfStateEstimator(StateEstimator):
has no graph to throttle); it integrates every anchor as a
regular measurement.
"""
ts_ns = _datetime_to_ns(pose.timestamp)
ts_ns = int(pose.emitted_at)
self._guard_timestamp(ts_ns, source="pose_anchor")
meas_pose = _pose_se3_to_array(pose.pose_se3)
meas_pose = self._pose_estimate_to_matrix(pose)
# Both modes are treated identically by the ESKF — the
# JACOBIAN exclusion is iSAM2-graph-specific. AC-4.
@@ -641,6 +641,29 @@ class EskfStateEstimator(StateEstimator):
)
return WgsConverter.local_enu_to_latlonalt(origin, enu)
def _pose_estimate_to_matrix(self, pose: PoseEstimate) -> np.ndarray:
"""Convert a C4 :class:`PoseEstimate` to a 4x4 homogeneous matrix.
WGS84 → ENU via the injected origin (matches the iSAM2
estimator's helper); ``Quat`` → 3x3 rotation via the local
``_quat_to_rot`` helper applied to a freshly-built quaternion.
"""
origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN
try:
enu = WgsConverter.latlonalt_to_local_enu(origin, pose.position_wgs84)
except Exception as exc:
raise EstimatorDegradedError(
f"eskf add_pose_anchor: WGS84→ENU failed for frame {pose.frame_id}: {exc}"
) from exc
q = pose.orientation_world_T_body
rotation = _quat_to_rot(
np.array([float(q.w), float(q.x), float(q.y), float(q.z)], dtype=np.float64)
)
matrix = np.eye(4, dtype=np.float64)
matrix[:3, :3] = rotation
matrix[:3, 3] = enu
return matrix
def _compute_last_anchor_age_ms(self, now_ns: int) -> int:
if self._last_anchor_ns == 0:
return now_ns // 1_000_000