feat(lab): add recorded realtime spatial playback

src/k1link/perception/spatial_evidence.py

@@ -0,0 +1,118 @@
+"""Deterministic projections shared by recorded spatial evidence producers."""
+
+from __future__ import annotations
+
+import math
+from collections.abc import Mapping, Sequence
+
+import numpy as np
+import numpy.typing as npt
+
+from .threat import ReplayBodyFrame
+
+FloatArray = npt.NDArray[np.float64]
+
+
+class SpatialEvidenceProjectionError(RuntimeError):
+    """Recorded spatial evidence cannot be projected without changing meaning."""
+
+
+def sample_points_in_body_frame(
+    points_map: FloatArray,
+    body_frame: ReplayBodyFrame,
+    *,
+    point_limit: int,
+) -> tuple[list[list[float]], int]:
+    """Project one immutable map-frame increment into the current body frame."""
+
+    if point_limit < 1:
+        raise SpatialEvidenceProjectionError("spatial evidence point limit must be positive")
+    points = np.asarray(points_map, dtype=np.float64)
+    if points.ndim != 2 or points.shape[1] != 3 or not np.isfinite(points).all():
+        raise SpatialEvidenceProjectionError("spatial evidence point array is invalid")
+    basis = np.asarray(body_frame.basis_map_from_body, dtype=np.float64)
+    origin = np.asarray(body_frame.origin_map_xyz_m, dtype=np.float64)
+    if basis.shape != (3, 3) or origin.shape != (3,):
+        raise SpatialEvidenceProjectionError("spatial evidence body frame is invalid")
+    points_body = (points - origin) @ basis
+    stride = max(1, math.ceil(points_body.shape[0] / point_limit))
+    sampled = points_body[::stride][:point_limit]
+    return np.round(sampled, 6).tolist(), int(points.shape[0])
+
+
+def project_metric_obstacles_to_body(
+    metric_rows: Sequence[Mapping[str, object]],
+    body_frame: ReplayBodyFrame,
+    *,
+    occupied_voxel_size_m: float,
+) -> list[dict[str, object]]:
+    """Project ledger-owned metric components without recomputing their decision."""
+
+    if not math.isfinite(occupied_voxel_size_m) or occupied_voxel_size_m <= 0:
+        raise SpatialEvidenceProjectionError("occupied voxel size is invalid")
+    visuals: list[dict[str, object]] = []
+    for row in metric_rows:
+        centroid = row.get("centroid_map_xyz_m")
+        cells = row.get("cells")
+        if not isinstance(centroid, list) or len(centroid) != 3 or not isinstance(cells, list):
+            continue
+        centroid_map = _finite_vector3(centroid, "metric centroid")
+        centroid_body = body_frame.map_point_to_body(centroid_map)
+        cell_centers: list[list[float]] = []
+        for raw_cell in cells:
+            if not isinstance(raw_cell, dict):
+                raise SpatialEvidenceProjectionError("occupied cell is invalid")
+            indices = (
+                _signed_integer(raw_cell.get("x"), "cell x"),
+                _signed_integer(raw_cell.get("y"), "cell y"),
+                _signed_integer(raw_cell.get("z"), "cell z"),
+            )
+            point_map = (
+                (indices[0] + 0.5) * occupied_voxel_size_m,
+                (indices[1] + 0.5) * occupied_voxel_size_m,
+                (indices[2] + 0.5) * occupied_voxel_size_m,
+            )
+            cell_centers.append(list(body_frame.map_point_to_body(point_map)))
+        visuals.append(
+            {
+                "component_id": row.get("component_id"),
+                "state": row.get("state"),
+                "motion": row.get("motion"),
+                "centroid_body_xyz_m": list(centroid_body),
+                "cell_centers_body_xyz_m": cell_centers,
+                "assessment": row.get("assessment"),
+            }
+        )
+    return visuals
+
+
+def _finite_vector3(value: Sequence[object], label: str) -> tuple[float, float, float]:
+    if len(value) != 3:
+        raise SpatialEvidenceProjectionError(f"{label} is invalid")
+    return (
+        _finite_float(value[0], label),
+        _finite_float(value[1], label),
+        _finite_float(value[2], label),
+    )
+
+
+def _finite_float(value: object, label: str) -> float:
+    if not isinstance(value, (int, float)) or isinstance(value, bool):
+        raise SpatialEvidenceProjectionError(f"{label} is invalid")
+    parsed = float(value)
+    if not math.isfinite(parsed):
+        raise SpatialEvidenceProjectionError(f"{label} is invalid")
+    return parsed
+
+
+def _signed_integer(value: object, label: str) -> int:
+    if not isinstance(value, int) or isinstance(value, bool):
+        raise SpatialEvidenceProjectionError(f"{label} is invalid")
+    return value
+
+
+__all__ = [
+    "SpatialEvidenceProjectionError",
+    "project_metric_obstacles_to_body",
+    "sample_points_in_body_frame",
+]