feat(perception): canonicalize metric geometry
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,496 @@
|
||||
"""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",
|
||||
]
|
||||
Reference in New Issue
Block a user