fix(perception): stabilize replay body frame

src/k1link/perception/geometry.py

@@ -38,9 +38,7 @@ from .recorded_source import RECORDED_SOURCE_PACK_ID, RecordedFrameReference
 
 GEOMETRY_PROFILE_SCHEMA: Final = "missioncore.geometry-association-profile/v1"
 GEOMETRY_PROVIDER_ID: Final = "ravnoves00-geometry-association/v1"
-DEFAULT_GEOMETRY_PROFILE_PATH: Final = Path(
-    "config/perception/m4-geometry-association-v1.json"
-)
+DEFAULT_GEOMETRY_PROFILE_PATH: Final = Path("config/perception/m4-geometry-association-v1.json")
 
 FloatArray = npt.NDArray[np.float64]
 UInt8Array = npt.NDArray[np.uint8]
@@ -85,6 +83,20 @@ class GeometryFrame:
         return int(self.points_map.shape[0])
 
 
+@dataclass(frozen=True, slots=True)
+class ReplayBodyFrameInputs:
+    """Verified inputs required to derive one replay-only virtual body frame."""
+
+    sensor_position_map: FloatArray
+    sensor_orientation_map_from_lidar_xyzw: FloatArray
+    ground_plane_coefficients_map: FloatArray
+    sensor_height_m: float
+    surface_slope_deg: float
+    trajectory_start_position_map: FloatArray
+    trajectory_end_position_map: FloatArray
+    t_camera_from_lidar: FloatArray
+
+
 @dataclass(frozen=True, slots=True)
 class GeometryProviderSnapshot:
     input_frames: int
@@ -279,13 +291,78 @@ class RecordedGeometryStore:
             ),
         )
 
+    def replay_body_frame_inputs(
+        self,
+        frame_id: str,
+        *,
+        trajectory_half_window_frames: int,
+    ) -> ReplayBodyFrameInputs | None:
+        """Return source-bound pose, surface and route evidence without inventing axes."""
+
+        if trajectory_half_window_frames < 1:
+            raise GeometryProviderError("trajectory half-window must be positive")
+        prefix = "frame-"
+        if not frame_id.startswith(prefix) or not frame_id[len(prefix) :].isdigit():
+            raise GeometryProviderError("replay body frame identity is invalid")
+        frame_index = int(frame_id[len(prefix) :])
+        if not 0 <= frame_index < self.profile.frame_count:
+            raise GeometryProviderError("replay body frame is outside the source profile")
+        if not bool(self._source["sample_available"][frame_index]) or not bool(
+            self._surface["frame_valid"][frame_index]
+        ):
+            return None
+        required = {
+            "plane_coefficients_map",
+            "sensor_height_m",
+            "slope_deg",
+        }
+        if not required.issubset(self._surface):
+            raise GeometryProviderError("local surface lacks replay body-frame evidence")
+        first = max(0, frame_index - trajectory_half_window_frames)
+        last = min(self.profile.frame_count, frame_index + trajectory_half_window_frames + 1)
+        available = np.flatnonzero(self._source["sample_available"][first:last]) + first
+        if available.size == 0:
+            return None
+        values = ReplayBodyFrameInputs(
+            sensor_position_map=np.asarray(
+                self._source["pose_positions_map"][frame_index], dtype=np.float64
+            ),
+            sensor_orientation_map_from_lidar_xyzw=np.asarray(
+                self._source["pose_quaternions_map_from_lidar"][frame_index],
+                dtype=np.float64,
+            ),
+            ground_plane_coefficients_map=np.asarray(
+                self._surface["plane_coefficients_map"][frame_index],
+                dtype=np.float64,
+            ),
+            sensor_height_m=float(self._surface["sensor_height_m"][frame_index]),
+            surface_slope_deg=float(self._surface["slope_deg"][frame_index]),
+            trajectory_start_position_map=np.asarray(
+                self._source["pose_positions_map"][int(available[0])], dtype=np.float64
+            ),
+            trajectory_end_position_map=np.asarray(
+                self._source["pose_positions_map"][int(available[-1])], dtype=np.float64
+            ),
+            t_camera_from_lidar=np.asarray(self._source["t_camera_from_lidar"], dtype=np.float64),
+        )
+        if not all(
+            np.isfinite(value).all()
+            for value in (
+                values.sensor_position_map,
+                values.sensor_orientation_map_from_lidar_xyzw,
+                values.ground_plane_coefficients_map,
+                values.trajectory_start_position_map,
+                values.trajectory_end_position_map,
+                values.t_camera_from_lidar,
+            )
+        ) or not math.isfinite(values.sensor_height_m + values.surface_slope_deg):
+            raise GeometryProviderError("replay body-frame evidence is not finite")
+        return values
+
     def available_frame_indices(self) -> tuple[int, ...]:
         """Expose the immutable availability partition for deterministic sampling."""
 
-        return tuple(
-            int(index)
-            for index in np.flatnonzero(self._source["sample_available"])
-        )
+        return tuple(int(index) for index in np.flatnonzero(self._source["sample_available"]))
 
     def _validate(self) -> None:
         source_required = {