mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 07: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:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user