fix(perception): stabilize replay body frame

tests/test_replay_threat_provider.py

@@ -2,6 +2,8 @@ from __future__ import annotations
 
 from pathlib import Path
 
+import pytest
+
 from k1link.perception.contracts import (
     BoundingRegion2D,
     CorridorIntersection,
@@ -15,23 +17,29 @@ from k1link.perception.contracts import (
     TemporalState,
     ThreatDecision,
 )
+from k1link.perception.geometry import RecordedGeometryStore
 from k1link.perception.graph_validation import validate_threats
 from k1link.perception.threat import (
     DualEvidenceReplayThreatProvider,
-    ReplayPose,
+    RecordedReplayBodyFrameResolver,
+    ReplayBodyFrame,
     load_replay_threat_profile,
 )
 
 REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
-PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-replay-threat-v1.json"
+PROFILE_PATH = REPOSITORY_ROOT / "config/perception/m4-replay-threat-v2.json"
 
 
-class _Poses:
-    def pose_for_frame(self, frame_id: str) -> ReplayPose:
-        return ReplayPose(
+class _BodyFrames:
+    def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame:
+        return ReplayBodyFrame(
             frame_id=frame_id,
-            position_map_xyz_m=(0.0, 0.0, 0.0),
-            orientation_map_from_lidar_xyzw=(0.0, 0.0, 0.0, 1.0),
+            origin_map_xyz_m=(0.0, 0.0, 0.0),
+            basis_map_from_body=((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)),
+            sensor_height_m=1.25,
+            surface_slope_deg=0.0,
+            forward_source="fixture",
+            camera_forward_alignment_deg=0.0,
         )
 
 
@@ -63,9 +71,7 @@ def _obstacle(
         last_centroid_xyz_m=None if state is TemporalState.EXPIRED else current.centroid_xyz_m,
         motion=MotionState.UNKNOWN if state is not TemporalState.CURRENT else motion,
         motion_confidence=(
-            0.0
-            if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN
-            else 1.0
+            0.0 if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN else 1.0
         ),
         motion_reason=(
             "stale-support"
@@ -127,9 +133,35 @@ def test_replay_threat_profile_freezes_virtual_authority_and_dual_evidence_polic
     )
 
 
+def test_recorded_body_frame_is_grounded_and_does_not_inherit_handheld_roll_pitch() -> None:
+    profile = load_replay_threat_profile(PROFILE_PATH)
+    resolver = RecordedReplayBodyFrameResolver(
+        RecordedGeometryStore.from_repository(REPOSITORY_ROOT),
+        profile=profile.body_frame,
+    )
+
+    start = resolver.body_frame_for_frame("frame-000000")
+    middle = resolver.body_frame_for_frame("frame-000138")
+    later = resolver.body_frame_for_frame("frame-000274")
+
+    assert start is None  # the opening surface height is not qualified evidence
+    assert middle is not None and later is not None
+    assert tuple(row[2] for row in middle.basis_map_from_body) == (0.0, 0.0, 1.0)
+    assert tuple(row[2] for row in later.basis_map_from_body) == (0.0, 0.0, 1.0)
+    assert middle.sensor_height_m == pytest.approx(1.2509065924)
+    assert later.sensor_height_m == pytest.approx(1.2838213430)
+    assert middle.camera_forward_alignment_deg < 7.0
+    assert later.camera_forward_alignment_deg < 2.0
+
+    summary = resolver.qualification_summary()
+    assert summary["available"] == 3928
+    assert summary["qualified"] == 3861
+    assert summary["rejected"] == 67
+
+
 def test_static_crossing_approaching_and_geometry_only_critical_cases_are_never_safe() -> None:
     provider = DualEvidenceReplayThreatProvider(
-        pose_resolver=_Poses(),
+        body_frame_resolver=_BodyFrames(),
         profile=load_replay_threat_profile(PROFILE_PATH),
     )
     current_frame = "frame-000002"
@@ -179,17 +211,17 @@ def test_static_crossing_approaching_and_geometry_only_critical_cases_are_never_
     assert {item.decision for item in result} == {ThreatDecision.THREAT}
     assert all(item.corridor_intersection is CorridorIntersection.INTERSECTS for item in result)
     assert (
-        next(item for item in result if item.component_id == "approaching").ttc_seconds
-        is not None
+        next(item for item in result if item.component_id == "approaching").ttc_seconds is not None
+    )
+    assert (
+        "geometry-only-evidence"
+        in next(item for item in result if item.component_id == "geometry-only").reason_codes
     )
-    assert "geometry-only-evidence" in next(
-        item for item in result if item.component_id == "geometry-only"
-    ).reason_codes
 
 
 def test_receding_and_static_outside_are_clear_but_incomplete_evidence_is_unknown() -> None:
     provider = DualEvidenceReplayThreatProvider(
-        pose_resolver=_Poses(),
+        body_frame_resolver=_BodyFrames(),
         profile=load_replay_threat_profile(PROFILE_PATH),
     )
     current_frame = "frame-000002"
@@ -246,7 +278,7 @@ def test_receding_and_static_outside_are_clear_but_incomplete_evidence_is_unknow
 
 def test_semantic_hint_and_ephemeral_component_name_do_not_change_threat_geometry() -> None:
     provider = DualEvidenceReplayThreatProvider(
-        pose_resolver=_Poses(),
+        body_frame_resolver=_BodyFrames(),
         profile=load_replay_threat_profile(PROFILE_PATH),
     )
     history = (