"""Pure KB4 projection and frame-local occupied-geometry association. This is the product-owned extraction of the accepted E29/E32 mathematics. It has no LAB, compute-package, device-plugin or transport dependency. """ from __future__ import annotations import math from collections import deque from dataclasses import dataclass import numpy as np import numpy.typing as npt FloatArray = npt.NDArray[np.float64] IntArray = npt.NDArray[np.int64] UInt8Array = npt.NDArray[np.uint8] Float32Array = npt.NDArray[np.float32] POINT_SURFACE = 1 POINT_OCCUPIED = 2 POINT_BELOW_SURFACE = 3 class GeometryMathError(ValueError): """A projection or association input violates the frozen M4 contract.""" @dataclass(frozen=True, slots=True) class GeometryAssociationProfile: bbox_inset_fraction: float depth_cluster_minimum_gap_m: float depth_cluster_gap_fraction: float spatial_cluster_radius_m: float semantic_minimum_occupied_points: int semantic_minimum_occupied_voxels: int semantic_voxel_size_m: float conflict_minimum_classified_points: int conflict_surface_fraction: float geometry_local_radius_m: float geometry_voxel_size_m: float geometry_minimum_cluster_points: int geometry_minimum_cluster_voxels: int maximum_geometry_clusters_per_frame: int def __post_init__(self) -> None: numeric = ( self.bbox_inset_fraction, self.depth_cluster_minimum_gap_m, self.depth_cluster_gap_fraction, self.spatial_cluster_radius_m, self.semantic_voxel_size_m, self.conflict_surface_fraction, self.geometry_local_radius_m, self.geometry_voxel_size_m, ) if ( not np.isfinite(numeric).all() or not 0.0 <= self.bbox_inset_fraction < 0.25 or not 0.05 <= self.depth_cluster_minimum_gap_m <= 5.0 or not 0.0 <= self.depth_cluster_gap_fraction <= 1.0 or not 0.05 <= self.spatial_cluster_radius_m <= 5.0 or not 1 <= self.semantic_minimum_occupied_points <= 64 or not 1 <= self.semantic_minimum_occupied_voxels <= 32 or not 0.05 <= self.semantic_voxel_size_m <= 2.0 or not 1 <= self.conflict_minimum_classified_points <= 256 or not 0.5 <= self.conflict_surface_fraction <= 1.0 or not 1.0 <= self.geometry_local_radius_m <= 100.0 or not 0.05 <= self.geometry_voxel_size_m <= 5.0 or not 1 <= self.geometry_minimum_cluster_points <= 256 or not 1 <= self.geometry_minimum_cluster_voxels <= 128 or not 1 <= self.maximum_geometry_clusters_per_frame <= 512 ): raise GeometryMathError("geometry association profile is invalid") @dataclass(frozen=True, slots=True) class Kb4ProjectionProfile: width: int height: int intrinsic_fx_fy_cx_cy: tuple[float, float, float, float] distortion_kb4: tuple[float, float, float, float] t_camera_from_lidar: FloatArray def __post_init__(self) -> None: transform = np.asarray(self.t_camera_from_lidar, dtype=np.float64) values = (*self.intrinsic_fx_fy_cx_cy, *self.distortion_kb4) if ( self.width < 1 or self.height < 1 or transform.shape != (4, 4) or not np.isfinite(transform).all() or not np.isfinite(values).all() or self.intrinsic_fx_fy_cx_cy[0] <= 0.0 or self.intrinsic_fx_fy_cx_cy[1] <= 0.0 ): raise GeometryMathError("KB4 projection profile is invalid") frozen = np.array(transform, dtype=np.float64, copy=True) frozen.setflags(write=False) object.__setattr__(self, "t_camera_from_lidar", frozen) @dataclass(frozen=True, slots=True) class ProjectedPointCloud: pixels_xy: FloatArray depths_m: FloatArray source_indices: IntArray source_point_count: int camera_front_point_count: int @property def projected_point_count(self) -> int: return int(self.pixels_xy.shape[0]) @property def overlap_bounds_xyxy(self) -> tuple[float, float, float, float] | None: if not self.projected_point_count: return None return ( float(np.min(self.pixels_xy[:, 0])), float(np.min(self.pixels_xy[:, 1])), float(np.max(self.pixels_xy[:, 0])), float(np.max(self.pixels_xy[:, 1])), ) @dataclass(frozen=True, slots=True) class SemanticGeometrySupport: projected_points_in_region: int classified_points_in_region: int surface_points_in_region: int occupied_points_in_region: int below_surface_points_in_region: int occupied_source_indices: IntArray occupied_depths_m: FloatArray qualified: bool conflict: bool overlaps_projected_extent: bool @dataclass(frozen=True, slots=True) class GeometryCluster: source_indices: IntArray centroid_map_xyz_m: tuple[float, float, float] covariance_diagonal_m2: tuple[float, float, float] nearest_range_m: float voxel_count: int def project_map_points_kb4( points_map_xyz: npt.ArrayLike, *, position_map_xyz: npt.ArrayLike, orientation_map_from_lidar_xyzw: npt.ArrayLike, profile: Kb4ProjectionProfile, ) -> ProjectedPointCloud: """Project one registered map-frame increment into the raw KB4 image.""" points_map = _finite_points(points_map_xyz) position = np.asarray(position_map_xyz, dtype=np.float64) if position.shape != (3,) or not np.isfinite(position).all(): raise GeometryMathError("pose position must contain three finite values") rotation = quaternion_xyzw_to_rotation_matrix(orientation_map_from_lidar_xyzw) points_lidar = (points_map - position) @ rotation transform = profile.t_camera_from_lidar points_camera = points_lidar @ transform[:3, :3].T + transform[:3, 3] front = points_camera[:, 2] > 1e-6 front_indices = np.flatnonzero(front) front_points = points_camera[front] if front_points.size == 0: return ProjectedPointCloud( pixels_xy=np.empty((0, 2), dtype=np.float64), depths_m=np.empty(0, dtype=np.float64), source_indices=np.empty(0, dtype=np.int64), source_point_count=int(points_map.shape[0]), camera_front_point_count=0, ) x, y, z = front_points.T radial = np.hypot(x, y) theta = np.arctan2(radial, z) squared = theta * theta k1, k2, k3, k4 = profile.distortion_kb4 distorted = theta * ( 1.0 + k1 * squared + k2 * squared**2 + k3 * squared**3 + k4 * squared**4 ) scale = np.divide(distorted, radial, out=np.zeros_like(distorted), where=radial > 1e-12) fx, fy, cx, cy = profile.intrinsic_fx_fy_cx_cy u = fx * x * scale + cx v = fy * y * scale + cy in_frame = ( np.isfinite(u) & np.isfinite(v) & (u >= 0.0) & (u < profile.width) & (v >= 0.0) & (v < profile.height) ) return ProjectedPointCloud( pixels_xy=np.column_stack((u[in_frame], v[in_frame])).astype(np.float64, copy=False), depths_m=z[in_frame].astype(np.float64, copy=False), source_indices=front_indices[in_frame].astype(np.int64, copy=False), source_point_count=int(points_map.shape[0]), camera_front_point_count=int(front_points.shape[0]), ) def quaternion_xyzw_to_rotation_matrix(orientation_xyzw: npt.ArrayLike) -> FloatArray: quaternion = np.asarray(orientation_xyzw, dtype=np.float64) if quaternion.shape != (4,) or not np.isfinite(quaternion).all(): raise GeometryMathError("pose quaternion must contain four finite values") norm = float(np.linalg.norm(quaternion)) if not math.isfinite(norm) or norm < 1e-9: raise GeometryMathError("pose quaternion has no usable norm") x, y, z, w = quaternion / norm return np.asarray( ( (1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)), (2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)), (2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)), ), dtype=np.float64, ) def semantic_geometry_support( bbox_xyxy: tuple[float, float, float, float], *, projected: ProjectedPointCloud, frame_points_map: FloatArray, point_class: UInt8Array, profile: GeometryAssociationProfile, ) -> SemanticGeometrySupport: """Return qualified occupied support without assigning point ownership.""" bbox = np.asarray(bbox_xyxy, dtype=np.float64) if bbox.shape != (4,) or not np.isfinite(bbox).all() or np.any(bbox[2:] <= bbox[:2]): raise GeometryMathError("proposal region is invalid") width, height = float(bbox[2] - bbox[0]), float(bbox[3] - bbox[1]) inset = profile.bbox_inset_fraction inner = np.asarray( ( bbox[0] + width * inset, bbox[1] + height * inset, bbox[2] - width * inset, bbox[3] - height * inset, ), dtype=np.float64, ) pixels = projected.pixels_xy inside = ( (pixels[:, 0] >= inner[0]) & (pixels[:, 0] <= inner[2]) & (pixels[:, 1] >= inner[1]) & (pixels[:, 1] <= inner[3]) ) rows = np.flatnonzero(inside).astype(np.int64, copy=False) indices = projected.source_indices[rows] classes = point_class[indices] counts = np.bincount(classes, minlength=4) occupied_rows = rows[classes == POINT_OCCUPIED] clustered = _depth_cluster( occupied_rows, projected.depths_m, minimum_gap_m=profile.depth_cluster_minimum_gap_m, gap_fraction=profile.depth_cluster_gap_fraction, ) clustered = _spatial_cluster( clustered, projected.source_indices, frame_points_map, radius_m=profile.spatial_cluster_radius_m, ) occupied_indices = projected.source_indices[clustered].astype(np.int64, copy=False) occupied_depths = projected.depths_m[clustered].astype(np.float64, copy=False) voxel_count = _voxel_count( frame_points_map[occupied_indices], profile.semantic_voxel_size_m, ) occupied_count = int(occupied_indices.size) qualified = ( occupied_count >= profile.semantic_minimum_occupied_points and voxel_count >= profile.semantic_minimum_occupied_voxels ) classified = int(counts[POINT_SURFACE] + counts[POINT_OCCUPIED] + counts[POINT_BELOW_SURFACE]) conflict = ( not qualified and classified >= profile.conflict_minimum_classified_points and int(counts[POINT_OCCUPIED]) == 0 and float(counts[POINT_SURFACE] / max(1, classified)) >= profile.conflict_surface_fraction ) bounds = projected.overlap_bounds_xyxy bbox_tuple = (float(bbox[0]), float(bbox[1]), float(bbox[2]), float(bbox[3])) overlaps = bounds is not None and _regions_intersect(bbox_tuple, bounds) return SemanticGeometrySupport( projected_points_in_region=int(indices.size), classified_points_in_region=classified, surface_points_in_region=int(counts[POINT_SURFACE]), occupied_points_in_region=int(counts[POINT_OCCUPIED]), below_surface_points_in_region=int(counts[POINT_BELOW_SURFACE]), occupied_source_indices=occupied_indices, occupied_depths_m=occupied_depths, qualified=qualified, conflict=conflict, overlaps_projected_extent=overlaps, ) def geometry_only_clusters( *, points_map: FloatArray, point_class: UInt8Array, sensor_position_map: FloatArray, claimed_source_indices: frozenset[int], profile: GeometryAssociationProfile, ) -> tuple[GeometryCluster, ...]: """Return bounded unclaimed occupied components with no semantic class.""" occupied = np.flatnonzero(point_class == POINT_OCCUPIED).astype(np.int64) if occupied.size == 0: return () ranges = np.linalg.norm(points_map[occupied] - sensor_position_map, axis=1) occupied = occupied[ranges <= profile.geometry_local_radius_m] clusters: list[GeometryCluster] = [] for indices, voxel_count in _voxel_components( points_map[occupied], occupied, profile.geometry_voxel_size_m, ): if ( indices.size < profile.geometry_minimum_cluster_points or voxel_count < profile.geometry_minimum_cluster_voxels or any(int(value) in claimed_source_indices for value in indices) ): continue values = points_map[indices] distances = np.linalg.norm(values - sensor_position_map, axis=1) centroid = np.median(values, axis=0) covariance = values.var(axis=0) clusters.append( GeometryCluster( source_indices=indices.astype(np.int64, copy=False), centroid_map_xyz_m=( float(centroid[0]), float(centroid[1]), float(centroid[2]), ), covariance_diagonal_m2=( float(covariance[0]), float(covariance[1]), float(covariance[2]), ), nearest_range_m=float(np.min(distances)), voxel_count=voxel_count, ) ) clusters.sort(key=lambda item: (item.nearest_range_m, -int(item.source_indices.size))) return tuple(clusters[: profile.maximum_geometry_clusters_per_frame]) def _depth_cluster( rows: IntArray, depths: FloatArray, *, minimum_gap_m: float, gap_fraction: float, ) -> IntArray: if rows.size < 2: return rows ordered = rows[np.argsort(depths[rows])] groups: list[IntArray] = [] start = 0 for offset, gap in enumerate(np.diff(depths[ordered]), start=1): threshold = max(minimum_gap_m, gap_fraction * float(depths[ordered[offset - 1]])) if float(gap) > threshold: groups.append(ordered[start:offset]) start = offset groups.append(ordered[start:]) return min(groups, key=lambda group: (-int(group.size), float(np.median(depths[group])))) def _spatial_cluster( rows: IntArray, source_indices: IntArray, points_map: FloatArray, *, radius_m: float, ) -> IntArray: if rows.size < 2: return rows points = points_map[source_indices[rows]] adjacent = np.sum((points[:, None, :] - points[None, :, :]) ** 2, axis=2) <= radius_m**2 unseen = set(range(rows.size)) groups: list[list[int]] = [] while unseen: seed = unseen.pop() group, pending = [seed], [seed] while pending: current = pending.pop() connected = [item for item in tuple(unseen) if adjacent[current, item]] for item in connected: unseen.remove(item) pending.append(item) group.append(item) groups.append(group) selected = min( groups, key=lambda group: ( -len(group), float(np.median(np.linalg.norm(points[np.asarray(group, dtype=np.int64)], axis=1))), ), ) return rows[np.asarray(selected, dtype=np.int64)] def _voxel_components( points: FloatArray, source_indices: IntArray, voxel_size_m: float, ) -> list[tuple[IntArray, int]]: cells = np.floor(points / voxel_size_m).astype(np.int64) cell_points: dict[tuple[int, int, int], list[int]] = {} for local_index, cell in enumerate(cells): key = (int(cell[0]), int(cell[1]), int(cell[2])) cell_points.setdefault(key, []).append(int(source_indices[local_index])) remaining = set(cell_points) neighbors = tuple( (dx, dy, dz) for dx in (-1, 0, 1) for dy in (-1, 0, 1) for dz in (-1, 0, 1) if (dx, dy, dz) != (0, 0, 0) ) components: list[tuple[IntArray, int]] = [] while remaining: seed = remaining.pop() queue = deque([seed]) component = [seed] while queue: current = queue.popleft() for delta in neighbors: candidate = tuple(current[index] + delta[index] for index in range(3)) if candidate in remaining: remaining.remove(candidate) queue.append(candidate) component.append(candidate) indices = np.asarray( [index for cell in component for index in cell_points[cell]], dtype=np.int64, ) components.append((indices, len(component))) return components def _voxel_count(points: FloatArray, voxel_size_m: float) -> int: if points.size == 0: return 0 cells = np.floor(points / voxel_size_m).astype(np.int64) return int(np.unique(cells, axis=0).shape[0]) def _regions_intersect( left: tuple[float, float, float, float], right: tuple[float, float, float, float], ) -> bool: return ( left[0] <= right[2] and left[2] >= right[0] and left[1] <= right[3] and left[3] >= right[1] ) def _finite_points(points_xyz: npt.ArrayLike) -> FloatArray: points = np.asarray(points_xyz, dtype=np.float64) if points.ndim != 2 or points.shape[1:] != (3,) or not np.isfinite(points).all(): raise GeometryMathError("point cloud must be finite with shape (N, 3)") return points __all__ = [ "GeometryAssociationProfile", "GeometryCluster", "GeometryMathError", "Kb4ProjectionProfile", "POINT_BELOW_SURFACE", "POINT_OCCUPIED", "POINT_SURFACE", "ProjectedPointCloud", "SemanticGeometrySupport", "geometry_only_clusters", "project_map_points_kb4", "quaternion_xyzw_to_rotation_matrix", "semantic_geometry_support", ]