"""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 .geometry_math import Kb4ProjectionProfile, project_map_points_kb4 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 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") 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_camera", "project_metric_obstacles_to_body", "sample_points_in_body_frame", ]