feat(lab): project system obstacles into fisheye

src/k1link/perception/m48s_replay_timeline.py

@@ -19,7 +19,11 @@ import numpy as np
 from .geometry import RecordedGeometryStore
 from .geometry_math import project_map_points_kb4
 from .recorded_source import RECORDED_REPRESENTATION_ID
-from .spatial_evidence import project_metric_obstacles_to_body, sample_points_in_body_frame
+from .spatial_evidence import (
+    project_metric_obstacles_to_body,
+    project_metric_obstacles_to_camera,
+    sample_points_in_body_frame,
+)
 from .threat import (
     DEFAULT_REPLAY_THREAT_PROFILE_PATH,
     RecordedReplayBodyFrameResolver,
@@ -183,6 +187,11 @@ class M48sReplayTimeline:
                 if self.frame_diff_path is not None
                 else None
             ),
+            "camera_obstacle_projection_delivery": (
+                "factory-kb4-occupied-voxel-bounds"
+                if self.frame_diff_path is not None
+                else None
+            ),
             "world_state_frame_count": len(self.index.offsets_by_sequence),
             "superseded_frame_count": sum(
                 value == "superseded" for value in self.outcomes.values()
@@ -383,6 +392,7 @@ class M48sReplayTimeline:
                 )
 
         metric_visuals: list[dict[str, object]] = []
+        metric_rows: list[dict[str, object]] = []
         camera_proposals: list[dict[str, object]] = []
         assessments: list[dict[str, object]] = []
         if row is not None:
@@ -401,7 +411,6 @@ class M48sReplayTimeline:
                 _text(item.get("component_id"), "assessment component"): item
                 for item in assessments
             }
-            metric_rows: list[dict[str, object]] = []
             for obstacle in (
                 *_objects(obstacle_map.get("occupied"), "occupied obstacles"),
                 *_objects(obstacle_map.get("unknown"), "unknown obstacles"),
@@ -427,11 +436,26 @@ class M48sReplayTimeline:
                     occupied_voxel_size_m=self.profile.corridor.occupied_voxel_size_m,
                 )
                 provenance = self._component_provenance(sequence)
-                for visual in metric_visuals:
-                    visual["occupancy_source"] = provenance.get(
-                        str(visual["component_id"]),
-                        "baseline",
+                camera_projections = (
+                    {}
+                    if frame is None or not provenance
+                    else project_metric_obstacles_to_camera(
+                        metric_rows,
+                        position_map_xyz=frame.sensor_position_map,
+                        orientation_map_from_lidar_xyzw=frame.sensor_orientation_xyzw,
+                        profile=frame.projection,
+                        occupied_voxel_size_m=(
+                            self.profile.corridor.occupied_voxel_size_m
+                        ),
+                        component_ids=set(provenance),
                     )
+                )
+                for visual in metric_visuals:
+                    component_id = str(visual["component_id"])
+                    visual["occupancy_source"] = provenance.get(component_id, "baseline")
+                    projection = camera_projections.get(component_id)
+                    if projection is not None:
+                        visual["camera_projection"] = projection
             associated = set(_strings(row.get("associated_proposal_ids"), "associated ids"))
             for proposal in _objects(row.get("detector_proposals"), "detector proposals"):
                 proposal_id = _text(proposal.get("proposal_id"), "proposal id")

src/k1link/perception/spatial_evidence.py

@@ -8,6 +8,7 @@ from collections.abc import Mapping, Sequence
 import numpy as np
 import numpy.typing as npt
 
+from .geometry_math import Kb4ProjectionProfile, project_map_points_kb4
 from .threat import ReplayBodyFrame
 
 FloatArray = npt.NDArray[np.float64]
@@ -86,6 +87,96 @@ def project_metric_obstacles_to_body(
     return visuals
 
 
+def project_metric_obstacles_to_camera(
+    metric_rows: Sequence[Mapping[str, object]],
+    *,
+    position_map_xyz: npt.ArrayLike,
+    orientation_map_from_lidar_xyzw: npt.ArrayLike,
+    profile: Kb4ProjectionProfile,
+    occupied_voxel_size_m: float,
+    component_ids: set[str] | None = None,
+) -> dict[str, dict[str, object]]:
+    """Project ledger-owned occupied voxel bounds into the native KB4 frame.
+
+    This is a visualization projection of already accepted world-state
+    components.  It neither reclusters geometry nor changes threat authority.
+    """
+
+    if not math.isfinite(occupied_voxel_size_m) or occupied_voxel_size_m <= 0:
+        raise SpatialEvidenceProjectionError("occupied voxel size is invalid")
+    points: list[tuple[float, float, float]] = []
+    point_owners: list[tuple[str, int]] = []
+    for row in metric_rows:
+        component_id = row.get("component_id")
+        cells = row.get("cells")
+        if (
+            not isinstance(component_id, str)
+            or not component_id
+            or (component_ids is not None and component_id not in component_ids)
+            or not isinstance(cells, list)
+        ):
+            continue
+        for cell_index, raw_cell in enumerate(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"),
+            )
+            bounds = tuple(
+                (index * occupied_voxel_size_m, (index + 1) * occupied_voxel_size_m)
+                for index in indices
+            )
+            for x in bounds[0]:
+                for y in bounds[1]:
+                    for z in bounds[2]:
+                        points.append((x, y, z))
+                        point_owners.append((component_id, cell_index))
+    if not points:
+        return {}
+    projected = project_map_points_kb4(
+        points,
+        position_map_xyz=position_map_xyz,
+        orientation_map_from_lidar_xyzw=orientation_map_from_lidar_xyzw,
+        profile=profile,
+    )
+    grouped_pixels: dict[str, list[npt.NDArray[np.float64]]] = {}
+    grouped_depths: dict[str, list[float]] = {}
+    grouped_cells: dict[str, set[int]] = {}
+    for pixel, depth, source_index in zip(
+        projected.pixels_xy,
+        projected.depths_m,
+        projected.source_indices,
+        strict=True,
+    ):
+        component_id, cell_index = point_owners[int(source_index)]
+        grouped_pixels.setdefault(component_id, []).append(pixel)
+        grouped_depths.setdefault(component_id, []).append(float(depth))
+        grouped_cells.setdefault(component_id, set()).add(cell_index)
+    result: dict[str, dict[str, object]] = {}
+    for component_id, pixel_values in grouped_pixels.items():
+        pixels = np.asarray(pixel_values, dtype=np.float64)
+        depths = np.asarray(grouped_depths[component_id], dtype=np.float64)
+        if pixels.size == 0 or depths.size == 0:
+            continue
+        minimum = np.min(pixels, axis=0)
+        maximum = np.max(pixels, axis=0)
+        result[component_id] = {
+            "bbox_xyxy": [
+                round(float(minimum[0]), 3),
+                round(float(minimum[1]), 3),
+                round(float(maximum[0]), 3),
+                round(float(maximum[1]), 3),
+            ],
+            "nearest_depth_m": round(float(np.min(depths)), 6),
+            "projected_cell_count": len(grouped_cells[component_id]),
+            "projection": "factory-kb4-occupied-voxel-bounds",
+            "authority": "visual-derived",
+        }
+    return result
+
+
 def _finite_vector3(value: Sequence[object], label: str) -> tuple[float, float, float]:
     if len(value) != 3:
         raise SpatialEvidenceProjectionError(f"{label} is invalid")
@@ -113,6 +204,7 @@ def _signed_integer(value: object, label: str) -> int:
 
 __all__ = [
     "SpatialEvidenceProjectionError",
+    "project_metric_obstacles_to_camera",
     "project_metric_obstacles_to_body",
     "sample_points_in_body_frame",
 ]