feat(lab): project system obstacles into fisheye

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",
 ]