feat(lab): add recorded realtime spatial playback

src/k1link/perception/threat_replay.py

@@ -38,6 +38,10 @@ from .geometry import RecordedGeometryStore
 from .geometry_replay import GeometryReplayResult, read_geometry_replay_result
 from .providers import SourcePacket
 from .recorded_source import RecordedRavnoves00Source, ReplayPacing
+from .spatial_evidence import (
+    project_metric_obstacles_to_body,
+    sample_points_in_body_frame,
+)
 from .temporal_replay import TemporalReplayResult, read_temporal_replay_result
 from .threat import (
     DEFAULT_REPLAY_THREAT_PROFILE_PATH,
@@ -654,49 +658,24 @@ def _visual_frame(
     points = store.current_points(packet)
     if points is None:
         raise ThreatReplayError("visual frame has no current point cloud")
-    basis = np.asarray(body_frame.basis_map_from_body, dtype=np.float64)
-    origin = np.asarray(body_frame.origin_map_xyz_m, dtype=np.float64)
-    points_body = (points - origin) @ basis
-    stride = max(1, math.ceil(points_body.shape[0] / VISUAL_POINT_LIMIT))
-    sampled = points_body[::stride][:VISUAL_POINT_LIMIT]
-    metric_visuals = []
-    for row in metric_rows:
-        centroid = row.get("centroid_map_xyz_m")
-        cells = row.get("cells")
-        if not isinstance(centroid, list) or not isinstance(cells, list):
-            continue
-        centroid_body = body_frame.map_point_to_body(
-            (float(centroid[0]), float(centroid[1]), float(centroid[2]))
-        )
-        cell_centers = []
-        for raw_cell in cells:
-            cell = _object(raw_cell, "visual occupied cell")
-            point_map = tuple(
-                (_signed_integer(cell.get(key), f"cell {key}") + 0.5)
-                * profile.corridor.occupied_voxel_size_m
-                for key in ("x", "y", "z")
-            )
-            cell_centers.append(
-                list(body_frame.map_point_to_body((point_map[0], point_map[1], point_map[2])))
-            )
-        metric_visuals.append(
-            {
-                "component_id": row["component_id"],
-                "state": row["state"],
-                "motion": row["motion"],
-                "centroid_body_xyz_m": list(centroid_body),
-                "cell_centers_body_xyz_m": cell_centers,
-                "assessment": row["assessment"],
-            }
-        )
+    sampled, source_count = sample_points_in_body_frame(
+        points,
+        body_frame,
+        point_limit=VISUAL_POINT_LIMIT,
+    )
+    metric_visuals = project_metric_obstacles_to_body(
+        metric_rows,
+        body_frame,
+        occupied_voxel_size_m=profile.corridor.occupied_voxel_size_m,
+    )
     return {
         "schema_version": THREAT_REPLAY_VISUAL_SCHEMA_V2,
         "sequence": packet.envelope.sequence,
         "frame_id": packet.envelope.frame_id,
         "source_time_ns": packet.envelope.timestamps.source_ns,
-        "point_cloud_body_xyz_m": np.round(sampled, 6).tolist(),
-        "point_cloud_source_count": int(points.shape[0]),
-        "point_cloud_sample_count": int(sampled.shape[0]),
+        "point_cloud_body_xyz_m": sampled,
+        "point_cloud_source_count": source_count,
+        "point_cloud_sample_count": len(sampled),
         "point_cloud_layer": "current-increment",
         "rolling_map_component_count": sum(
             row.get("state") == TemporalState.RETAINED.value for row in metric_rows