211 lines
8.1 KiB
Python
211 lines
8.1 KiB
Python
"""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",
|
|
]
|