497 lines
18 KiB
Python
497 lines
18 KiB
Python
"""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",
|
|
]
|