"""Standalone bounded LiDAR fusion primitives for LAB E10. The worker deliberately receives a content-addressed replay pack instead of a K1 session or device authority. This module contains only geometry and world state projection; it cannot discover, start, stop, or configure a scanner. """ from __future__ import annotations import hashlib import json import math import statistics from collections import defaultdict, deque from collections.abc import Mapping, Sequence from dataclasses import dataclass, replace from pathlib import Path from typing import Any import numpy as np import numpy.typing as npt PACK_SCHEMA = "missioncore.e10-lidar-replay-pack/v1" WORLD_STATE_SCHEMA = "missioncore.live-perception-world-state/v1" IMAGE_WIDTH = 800 IMAGE_HEIGHT = 600 FloatArray = npt.NDArray[np.float64] def canonical_json(value: object) -> bytes: return json.dumps( value, ensure_ascii=False, sort_keys=True, separators=(",", ":"), allow_nan=False, ).encode() def sha256(path: Path) -> str: digest = hashlib.sha256() with path.open("rb") as stream: while chunk := stream.read(1024 * 1024): digest.update(chunk) return digest.hexdigest() @dataclass(frozen=True, slots=True) class ProjectionProfile: 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 @dataclass(frozen=True, slots=True) class Cuboid: center_map: tuple[float, float, float] half_size: tuple[float, float, float] quaternion_xyzw: tuple[float, float, float, float] @dataclass(frozen=True, slots=True) class CompletedCuboid: cuboid: Cuboid completion_fraction: float ground_z_map: float orientation_source: str temporal_status: str support_coverage_fraction: float @dataclass(slots=True) class _CuboidTrackState: center: FloatArray size: FloatArray yaw: float last_seconds: float hits: int @dataclass(frozen=True, slots=True) class TrackFusion: track_id: int label: str association_group: str score: float bbox_xyxy: tuple[float, float, float, float] candidate_points: int semantic_points: int clustered_points: int distance_p10_m: float | None distance_median_m: float | None distance_smoothed_m: float | None status: str cuboid: Cuboid | None source_indices: npt.NDArray[np.int64] pre_ground_clustered_points: int = 0 ground_rejected_points: int = 0 support_ground_z_map: float | None = None geometry: str = "none" observed_cuboid: Cuboid | None = None completion_fraction: float | None = None ground_z_map: float | None = None orientation_source: str | None = None temporal_status: str | None = None support_coverage_fraction: float | None = None class LidarReplayPack: """Validated immutable LiDAR/pose replay input for one camera selection.""" def __init__(self, root: Path, *, expected_job_id: str) -> None: self.root = root.resolve(strict=True) manifest_path = self.root / "manifest.json" arrays_path = self.root / "lidar-pack.npz" manifest = json.loads(manifest_path.read_text(encoding="utf-8")) identity = manifest.get("identity") if isinstance(manifest, dict) else None identity_sha256 = manifest.get("identity_sha256") if isinstance(manifest, dict) else None if ( not isinstance(manifest, dict) or manifest.get("schema_version") != PACK_SCHEMA or not isinstance(identity, dict) or identity.get("schema_version") != PACK_SCHEMA or identity.get("job_id") != expected_job_id or not isinstance(identity_sha256, str) or hashlib.sha256(canonical_json(identity)).hexdigest() != identity_sha256 or self.root.name != f"e10-lidar-pack-{identity_sha256}" or manifest.get("pack_id") != self.root.name or manifest.get("artifact", {}).get("path") != arrays_path.name or manifest.get("artifact", {}).get("sha256") != sha256(arrays_path) or manifest.get("artifact", {}).get("byte_length") != arrays_path.stat().st_size ): raise RuntimeError("LAB E10 LiDAR replay pack identity is invalid") self.manifest = manifest self.identity = identity self.pack_id = self.root.name self.arrays_path = arrays_path self.arrays = np.load(arrays_path, allow_pickle=False) required = { "frame_indices", "source_frame_indices", "session_seconds", "sample_available", "cloud_offsets", "cloud_points_map", "pose_positions_map", "pose_quaternions_map_from_lidar", "lidar_camera_delta_ms", "pose_point_delta_ms", "intrinsic_fx_fy_cx_cy", "distortion_kb4", "t_camera_from_lidar", } if set(self.arrays.files) != required: raise RuntimeError("LAB E10 LiDAR replay pack array set changed") self.frame_indices = self.arrays["frame_indices"] self.source_frame_indices = self.arrays["source_frame_indices"] self.session_seconds = self.arrays["session_seconds"] self.sample_available = self.arrays["sample_available"] self.cloud_offsets = self.arrays["cloud_offsets"] self.cloud_points = self.arrays["cloud_points_map"] self.positions = self.arrays["pose_positions_map"] self.quaternions = self.arrays["pose_quaternions_map_from_lidar"] self.lidar_camera_delta_ms = self.arrays["lidar_camera_delta_ms"] self.pose_point_delta_ms = self.arrays["pose_point_delta_ms"] count = int(identity.get("frame_count", -1)) if ( count < 2 or self.frame_indices.dtype != np.int64 or not np.array_equal(self.frame_indices, np.arange(count, dtype=np.int64)) or self.source_frame_indices.shape != (count,) or self.session_seconds.shape != (count,) or self.sample_available.dtype != np.bool_ or self.sample_available.shape != (count,) or self.cloud_offsets.shape != (count + 1,) or self.cloud_points.ndim != 2 or self.cloud_points.shape[1:] != (3,) or self.positions.shape != (count, 3) or self.quaternions.shape != (count, 4) or self.lidar_camera_delta_ms.shape != (count,) or self.pose_point_delta_ms.shape != (count,) or int(self.cloud_offsets[0]) != 0 or int(self.cloud_offsets[-1]) != int(self.cloud_points.shape[0]) or np.any(np.diff(self.cloud_offsets) < 0) or not np.all(np.diff(self.session_seconds) > 0) ): raise RuntimeError("LAB E10 LiDAR replay pack shapes are invalid") self.profile = ProjectionProfile( width=int(identity["projection"]["width"]), height=int(identity["projection"]["height"]), intrinsic_fx_fy_cx_cy=tuple( float(value) for value in self.arrays["intrinsic_fx_fy_cx_cy"] ), distortion_kb4=tuple(float(value) for value in self.arrays["distortion_kb4"]), t_camera_from_lidar=np.asarray(self.arrays["t_camera_from_lidar"], dtype=np.float64), ) if ( self.profile.width != IMAGE_WIDTH or self.profile.height != IMAGE_HEIGHT or self.profile.t_camera_from_lidar.shape != (4, 4) ): raise RuntimeError("LAB E10 projection profile is incompatible") @property def frame_count(self) -> int: return int(self.frame_indices.shape[0]) def close(self) -> None: self.arrays.close() def frame(self, index: int) -> tuple[FloatArray, tuple[float, ...], tuple[float, ...]] | None: if not 0 <= index < self.frame_count: raise IndexError(index) if not bool(self.sample_available[index]): return None start = int(self.cloud_offsets[index]) end = int(self.cloud_offsets[index + 1]) position = tuple(float(value) for value in self.positions[index]) quaternion = tuple(float(value) for value in self.quaternions[index]) points = np.asarray(self.cloud_points[start:end], dtype=np.float64) if not np.isfinite(points).all() or not all( math.isfinite(value) for value in position + quaternion ): raise RuntimeError("LAB E10 LiDAR replay frame contains non-finite geometry") return points, position, quaternion def quaternion_to_rotation(orientation_xyzw: Sequence[float]) -> FloatArray: quaternion = np.asarray(orientation_xyzw, dtype=np.float64) norm = float(np.linalg.norm(quaternion)) if quaternion.shape != (4,) or not math.isfinite(norm) or norm < 1e-9: raise RuntimeError("LAB E10 pose quaternion is invalid") x, y, z, w = quaternion / norm return np.asarray( [ [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], ], dtype=np.float64, ) def map_points_to_lidar( points_map: FloatArray, position_map: Sequence[float], orientation_map_from_lidar: Sequence[float], ) -> FloatArray: return (points_map - np.asarray(position_map, dtype=np.float64)) @ quaternion_to_rotation( orientation_map_from_lidar ) def project_points( points_map: FloatArray, position_map: Sequence[float], orientation_map_from_lidar: Sequence[float], profile: ProjectionProfile, ) -> tuple[FloatArray, FloatArray, npt.NDArray[np.int64], FloatArray]: points_lidar = map_points_to_lidar(points_map, position_map, orientation_map_from_lidar) transform = profile.t_camera_from_lidar points_camera = points_lidar @ transform[:3, :3].T + transform[:3, 3] front = points_camera[:, 2] > 1e-6 source = np.flatnonzero(front) camera = points_camera[front] if camera.size == 0: return ( np.empty((0, 2), dtype=np.float64), np.empty((0,), dtype=np.float64), np.empty((0,), dtype=np.int64), points_lidar, ) x, y, z = camera.T radial = np.hypot(x, y) theta = np.arctan2(radial, z) theta2 = theta * theta k1, k2, k3, k4 = profile.distortion_kb4 distorted = theta * (1 + k1 * theta2 + k2 * theta2**2 + k3 * theta2**3 + k4 * theta2**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 keep = ( np.isfinite(u) & np.isfinite(v) & (u >= 0) & (u < profile.width) & (v >= 0) & (v < profile.height) ) pixels = np.column_stack((u[keep], v[keep])) depths = z[keep] indices = source[keep].astype(np.int64, copy=False) if depths.size: px = np.rint(pixels[:, 0]).astype(np.int64) py = np.rint(pixels[:, 1]).astype(np.int64) keys = py * profile.width + px order = np.lexsort((depths, keys)) ordered_keys = keys[order] first = np.concatenate(([True], ordered_keys[1:] != ordered_keys[:-1])) visible = np.sort(order[first]) pixels, depths, indices = pixels[visible], depths[visible], indices[visible] return pixels, depths, indices, points_lidar def _box_iou(one: Sequence[float], two: Sequence[float]) -> float: x1, y1 = max(one[0], two[0]), max(one[1], two[1]) x2, y2 = min(one[2], two[2]), min(one[3], two[3]) intersection = max(0.0, x2 - x1) * max(0.0, y2 - y1) union = max(0.0, one[2] - one[0]) * max(0.0, one[3] - one[1]) union += max(0.0, two[2] - two[0]) * max(0.0, two[3] - two[1]) - intersection return 0.0 if union <= 0 else intersection / union def _depth_cluster( indices: npt.NDArray[np.int64], depths: FloatArray, profile: Mapping[str, Any] ) -> npt.NDArray[np.int64]: if indices.size < 2: return indices ordered = indices[np.argsort(depths[indices])] groups: list[npt.NDArray[np.int64]] = [] start = 0 for offset, gap in enumerate(np.diff(depths[ordered]), start=1): threshold = max( float(profile["depth_cluster_minimum_gap_m"]), float(profile["depth_cluster_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( indices: npt.NDArray[np.int64], points_lidar: FloatArray, radius: float ) -> npt.NDArray[np.int64]: if indices.size < 2: return indices points = points_lidar[indices] adjacent = np.sum((points[:, None, :] - points[None, :, :]) ** 2, axis=2) <= radius * radius unseen = set(range(indices.size)) groups: list[list[int]] = [] while unseen: seed = unseen.pop() group = [seed] pending = [seed] while pending: current = pending.pop() for neighbor in [value for value in tuple(unseen) if adjacent[current, value]]: unseen.remove(neighbor) pending.append(neighbor) group.append(neighbor) groups.append(group) selected = min( groups, key=lambda group: (-len(group), float(np.median(np.linalg.norm(points[group], axis=1)))), ) return indices[np.asarray(selected, dtype=np.int64)] def _cuboid(points_map: FloatArray, profile: Mapping[str, Any], group: str) -> Cuboid | None: if points_map.shape[0] < 2: return None xy = points_map[:, :2] centered = xy - np.median(xy, axis=0) covariance = centered.T @ centered / max(1, centered.shape[0] - 1) values, vectors = np.linalg.eigh(covariance) axis = vectors[:, int(np.argmax(values))] yaw = math.atan2(float(axis[1]), float(axis[0])) forward = np.asarray([math.cos(yaw), math.sin(yaw)]) side = np.asarray([-math.sin(yaw), math.cos(yaw)]) local = np.column_stack((xy @ forward, xy @ side)) lower = np.percentile(local, 5.0, axis=0) upper = np.percentile(local, 95.0, axis=0) lower_z, upper_z = np.percentile(points_map[:, 2], [5.0, 95.0]) spans = np.asarray([upper[0] - lower[0], upper[1] - lower[1], upper_z - lower_z]) maximum = np.asarray(profile["maximum_oriented_extent_m"][group], dtype=np.float64) if ( not np.isfinite(spans).all() or np.any(spans > float(profile["maximum_cuboid_span_m"])) or np.any(spans > maximum) ): return None sizes = np.maximum(spans, float(profile["minimum_cuboid_extent_m"])) local_center = (lower + upper) * 0.5 center_xy = local_center[0] * forward + local_center[1] * side return Cuboid( center_map=( float(center_xy[0]), float(center_xy[1]), (float(lower_z) + float(upper_z)) * 0.5, ), half_size=tuple(float(value) for value in sizes * 0.5), quaternion_xyzw=(0.0, 0.0, math.sin(yaw * 0.5), math.cos(yaw * 0.5)), ) def _box_yaw(cuboid: Cuboid) -> float: z, w = cuboid.quaternion_xyzw[2:] return 2.0 * math.atan2(z, w) def _align_box_yaw(yaw: float, reference: float) -> float: """Align a cuboid yaw to the closest equivalent pi-periodic heading.""" return reference + ((yaw - reference + math.pi * 0.5) % math.pi) - math.pi * 0.5 def _completion_profile(profile: Mapping[str, Any]) -> dict[str, Any]: required_labels = {"person", "bicycle", "motorcycle", "car", "truck", "bus"} classes = profile.get("classes") ground = profile.get("ground") temporal = profile.get("temporal") orientation = profile.get("orientation") if ( profile.get("mode") != "class-prior-amodal-v1" or profile.get("failure_policy") not in {"reject", "visible-envelope"} or not isinstance(classes, Mapping) or set(classes) != required_labels or not all(isinstance(value, Mapping) for value in (ground, temporal, orientation)) ): raise RuntimeError("LAB E13 cuboid completion profile is invalid") for label, value in classes.items(): nominal = np.asarray(value.get("nominal_size_m"), dtype=np.float64) minimum = np.asarray(value.get("minimum_size_m"), dtype=np.float64) maximum = np.asarray(value.get("maximum_size_m"), dtype=np.float64) padding = np.asarray(value.get("support_padding_m"), dtype=np.float64) if ( nominal.shape != (3,) or minimum.shape != (3,) or maximum.shape != (3,) or padding.shape != (3,) or not np.isfinite(np.concatenate((nominal, minimum, maximum, padding))).all() or np.any(minimum <= 0) or np.any(padding < 0) or np.any(nominal < minimum) or np.any(nominal > maximum) ): raise RuntimeError(f"LAB E13 {label} size prior is invalid") scalar_checks = ( (ground, "local_radius_m", 0.5, 10.0), (ground, "lower_percentile", 0.0, 30.0), (ground, "maximum_below_support_m", 0.1, 3.0), (ground, "maximum_above_support_m", 0.0, 0.5), (ground, "fallback_below_support_m", 0.0, 1.0), (orientation, "minimum_anisotropy_ratio", 1.0, 20.0), (orientation, "face_width_switch_fraction", 0.5, 2.0), (temporal, "center_alpha", 0.01, 1.0), (temporal, "size_alpha", 0.01, 1.0), (temporal, "yaw_alpha", 0.01, 1.0), (temporal, "maximum_center_innovation_m", 0.1, 20.0), (temporal, "maximum_yaw_innovation_degrees", 1.0, 90.0), (temporal, "maximum_idle_s", 0.05, 10.0), (profile, "minimum_support_coverage_fraction", 0.5, 1.0), ) for owner, name, lower, upper in scalar_checks: try: value = float(owner[name]) except (KeyError, TypeError, ValueError) as exc: raise RuntimeError(f"LAB E13 cuboid completion field {name} is invalid") from exc if not lower <= value <= upper: raise RuntimeError(f"LAB E13 cuboid completion field {name} is invalid") if not 1 <= int(temporal.get("confirmation_hits", 0)) <= 20: raise RuntimeError("LAB E13 cuboid confirmation hit count is invalid") return dict(profile) def _object_support_ground_filter_profile( profile: Mapping[str, Any], ) -> dict[str, Any]: groups = {"person", "bicycle", "motorcycle", "vehicle"} minimum_height = profile.get("minimum_height_above_ground_m") maximum_height = profile.get("maximum_height_above_ground_m") if ( profile.get("mode") != "local-ground-relative-object-support-v1" or not isinstance(minimum_height, Mapping) or set(minimum_height) != groups or not isinstance(maximum_height, Mapping) or set(maximum_height) != groups ): raise RuntimeError("object-support ground filter profile is invalid") scalar_checks = ( ("local_radius_m", 0.5, 10.0), ("lower_percentile", 0.0, 30.0), ("maximum_below_support_m", 0.1, 3.0), ("maximum_above_support_m", 0.0, 0.5), ("fallback_below_support_m", 0.0, 1.0), ) for name, lower, upper in scalar_checks: try: value = float(profile[name]) except (KeyError, TypeError, ValueError) as exc: raise RuntimeError(f"object-support ground filter field {name} is invalid") from exc if not lower <= value <= upper: raise RuntimeError(f"object-support ground filter field {name} is invalid") for group in groups: try: lower = float(minimum_height[group]) upper = float(maximum_height[group]) except (KeyError, TypeError, ValueError) as exc: raise RuntimeError( f"object-support ground filter height for {group} is invalid" ) from exc if not 0.0 <= lower < upper <= 6.0: raise RuntimeError(f"object-support ground filter height for {group} is invalid") return dict(profile) def _local_ground_z( *, all_points_map: FloatArray, center_xy: FloatArray, support_lower_z: float, profile: Mapping[str, Any], ) -> float: radius = float(profile["local_radius_m"]) if all_points_map.size: distances = np.linalg.norm(all_points_map[:, :2] - center_xy, axis=1) candidates = all_points_map[distances <= radius, 2] else: candidates = np.empty(0, dtype=np.float64) estimate = ( float(np.percentile(candidates, float(profile["lower_percentile"]))) if candidates.size else math.nan ) if ( not math.isfinite(estimate) or estimate < support_lower_z - float(profile["maximum_below_support_m"]) or estimate > support_lower_z + float(profile["maximum_above_support_m"]) ): estimate = support_lower_z - float(profile["fallback_below_support_m"]) return estimate def _filter_object_support_by_ground( indices: npt.NDArray[np.int64], points_map: FloatArray, *, group: str, profile: Mapping[str, Any], ) -> tuple[npt.NDArray[np.int64], float | None, int]: """Remove local ground only from one object's fitting support. `points_map` is never modified or reduced. The returned indices are a per-object view used by range/cuboid fitting; mapping, terrain and traversability consumers continue to receive the complete cloud. """ if indices.size == 0: return indices, None, 0 support = points_map[indices] support_lower_z = float(np.percentile(support[:, 2], 5.0)) ground_z = _local_ground_z( all_points_map=points_map, center_xy=np.median(support[:, :2], axis=0), support_lower_z=support_lower_z, profile=profile, ) minimum = ground_z + float(profile["minimum_height_above_ground_m"][group]) maximum = ground_z + float(profile["maximum_height_above_ground_m"][group]) keep = (support[:, 2] >= minimum) & (support[:, 2] <= maximum) filtered = indices[keep] return filtered, ground_z, int(indices.size - filtered.size) def _support_overlap_fraction( one: npt.NDArray[np.int64], two: npt.NDArray[np.int64], ) -> float: if one.size == 0 or two.size == 0: return 0.0 shared = np.intersect1d(one, two, assume_unique=False).size return float(shared / min(one.size, two.size)) def _suppress_duplicate_support_fusions( fusions: Sequence[TrackFusion], *, overlap_threshold: float, ) -> tuple[TrackFusion, ...]: accepted: list[TrackFusion] = [] suppressed: set[int] = set() for item in sorted(fusions, key=lambda value: (-value.score, value.track_id)): if item.cuboid is None: continue if any( other.association_group == item.association_group and _support_overlap_fraction(other.source_indices, item.source_indices) >= overlap_threshold for other in accepted ): suppressed.add(item.track_id) else: accepted.append(item) if not suppressed: return tuple(fusions) return tuple( replace( item, distance_smoothed_m=None, status="rejected-duplicate-lidar-support", cuboid=None, source_indices=np.empty(0, dtype=np.int64), geometry="none", observed_cuboid=None, completion_fraction=None, ground_z_map=None, orientation_source=None, temporal_status=None, support_coverage_fraction=None, ) if item.track_id in suppressed else item for item in fusions ) class CuboidCompletionTracker: """Complete visible LiDAR surfaces into provenance-marked, smoothed cuboids. Completion is deliberately class-prior based. The returned box is useful for perception visualization and planning experiments, but it is never represented as fully measured geometry or ground truth. """ def __init__(self, profile: Mapping[str, Any]) -> None: self.profile = _completion_profile(profile) self.states: dict[tuple[int, str], _CuboidTrackState] = {} self.last_failure_reason: str | None = None def complete( self, *, track_id: int, label: str, support_points_map: FloatArray, all_points_map: FloatArray, sensor_position_map: Sequence[float], session_seconds: float, observed_cuboid: Cuboid, ) -> CompletedCuboid | None: self.last_failure_reason = None if support_points_map.shape[0] < 2 or support_points_map.shape[1:] != (3,): self.last_failure_reason = "insufficient-support" return None prior = self.profile["classes"].get(label) if not isinstance(prior, Mapping): self.last_failure_reason = "missing-class-prior" return None now = float(session_seconds) sensor = np.asarray(sensor_position_map, dtype=np.float64) if sensor.shape != (3,) or not np.isfinite(sensor).all() or not math.isfinite(now): self.last_failure_reason = "invalid-pose-or-time" return None self._prune(now) key = (int(track_id), label) previous = self.states.get(key) nominal = np.asarray(prior["nominal_size_m"], dtype=np.float64) observed_yaw = _box_yaw(observed_cuboid) orientation_source = "support-pca" centered = support_points_map[:, :2] - np.median(support_points_map[:, :2], axis=0) covariance = centered.T @ centered / max(1, centered.shape[0] - 1) values = np.linalg.eigvalsh(covariance) anisotropy = float(values[-1] / max(values[0], 1e-9)) anisotropy_minimum = float(self.profile["orientation"]["minimum_anisotropy_ratio"]) if anisotropy >= anisotropy_minimum and nominal[0] >= nominal[1] * 1.4: principal = np.asarray([math.cos(observed_yaw), math.sin(observed_yaw)]) projected = support_points_map[:, :2] @ principal principal_span = float(np.percentile(projected, 95.0) - np.percentile(projected, 5.0)) if principal_span <= nominal[1] * float( self.profile["orientation"]["face_width_switch_fraction"] ): observed_yaw += math.pi * 0.5 orientation_source = "support-face-normal" if previous is not None: observed_yaw = _align_box_yaw(observed_yaw, previous.yaw) if anisotropy < anisotropy_minimum: observed_yaw = previous.yaw orientation_source = "track-history" else: alternative = _align_box_yaw(observed_yaw + math.pi * 0.5, previous.yaw) if abs(alternative - previous.yaw) < abs(observed_yaw - previous.yaw): observed_yaw = alternative orientation_source = "track-history-axis-disambiguation" elif anisotropy < anisotropy_minimum: bearing = math.atan2( float(np.median(support_points_map[:, 1]) - sensor[1]), float(np.median(support_points_map[:, 0]) - sensor[0]), ) observed_yaw = bearing orientation_source = "sensor-bearing-fallback" forward = np.asarray([math.cos(observed_yaw), math.sin(observed_yaw)]) side = np.asarray([-math.sin(observed_yaw), math.cos(observed_yaw)]) local = np.column_stack( (support_points_map[:, :2] @ forward, support_points_map[:, :2] @ side) ) lower_xy = np.percentile(local, 5.0, axis=0) upper_xy = np.percentile(local, 95.0, axis=0) lower_z, upper_z = np.percentile(support_points_map[:, 2], [5.0, 95.0]) ground_z = self._ground_z( all_points_map=all_points_map, center_xy=np.median(support_points_map[:, :2], axis=0), support_lower_z=float(lower_z), ) minimum = np.asarray(prior["minimum_size_m"], dtype=np.float64) maximum = np.asarray(prior["maximum_size_m"], dtype=np.float64) padding = np.asarray(prior["support_padding_m"], dtype=np.float64) observed_size = np.asarray( [upper_xy[0] - lower_xy[0], upper_xy[1] - lower_xy[1], upper_z - lower_z], dtype=np.float64, ) required_size = np.asarray( [ observed_size[0] + 2.0 * padding[0], observed_size[1] + 2.0 * padding[1], max(0.0, float(upper_z) - ground_z) + padding[2], ], dtype=np.float64, ) size = np.clip(np.maximum.reduce((nominal, minimum, required_size)), minimum, maximum) if np.any(required_size > maximum + 1e-9) or not np.isfinite(size).all(): self.last_failure_reason = "required-size-exceeds-class-bound" return None sensor_local = np.asarray([sensor[:2] @ forward, sensor[:2] @ side], dtype=np.float64) center_local = np.asarray( [ self._amodal_axis_center( float(lower_xy[axis]), float(upper_xy[axis]), float(size[axis]), float(sensor_local[axis]), ) for axis in range(2) ], dtype=np.float64, ) center_xy = center_local[0] * forward + center_local[1] * side center = np.asarray([center_xy[0], center_xy[1], ground_z + size[2] * 0.5]) yaw = observed_yaw candidate_center = center.copy() candidate_size = size.copy() candidate_yaw = yaw temporal_status = "tentative" if previous is not None: temporal = self.profile["temporal"] center_innovation = float(np.linalg.norm(center - previous.center)) yaw_delta = _align_box_yaw(yaw, previous.yaw) - previous.yaw maximum_yaw = math.radians(float(temporal["maximum_yaw_innovation_degrees"])) if ( center_innovation > float(temporal["maximum_center_innovation_m"]) or abs(yaw_delta) > maximum_yaw ): temporal_status = "reset-innovation" hits = 1 else: center = previous.center + float(temporal["center_alpha"]) * ( center - previous.center ) size = previous.size + float(temporal["size_alpha"]) * (size - previous.size) yaw = previous.yaw + float(temporal["yaw_alpha"]) * yaw_delta hits = previous.hits + 1 temporal_status = ( "confirmed" if hits >= int(temporal["confirmation_hits"]) else "tentative" ) else: hits = 1 coverage = self._coverage(support_points_map, center, size, yaw) minimum_coverage = float(self.profile["minimum_support_coverage_fraction"]) if coverage < minimum_coverage and previous is not None: # A smoothed box must never drift away from its current measured support. center = candidate_center size = candidate_size yaw = candidate_yaw coverage = self._coverage(support_points_map, center, size, yaw) temporal_status = "reset-support-coverage" hits = 1 if coverage < minimum_coverage: self.last_failure_reason = "support-coverage-below-threshold" return None self.states[key] = _CuboidTrackState( center=center.copy(), size=size.copy(), yaw=float(yaw), last_seconds=now, hits=hits, ) observed_volume = float(np.prod(np.maximum(observed_size, 0.0))) completed_volume = float(np.prod(size)) completion_fraction = 1.0 - min(1.0, observed_volume / completed_volume) return CompletedCuboid( cuboid=Cuboid( center_map=tuple(float(value) for value in center), half_size=tuple(float(value) for value in size * 0.5), quaternion_xyzw=( 0.0, 0.0, math.sin(yaw * 0.5), math.cos(yaw * 0.5), ), ), completion_fraction=completion_fraction, ground_z_map=ground_z, orientation_source=orientation_source, temporal_status=temporal_status, support_coverage_fraction=coverage, ) def _ground_z( self, *, all_points_map: FloatArray, center_xy: FloatArray, support_lower_z: float, ) -> float: return _local_ground_z( all_points_map=all_points_map, center_xy=center_xy, support_lower_z=support_lower_z, profile=self.profile["ground"], ) @staticmethod def _amodal_axis_center(lower: float, upper: float, size: float, sensor: float) -> float: half = size * 0.5 allowed_lower = upper - half allowed_upper = lower + half if sensor < lower: return allowed_upper if sensor > upper: return allowed_lower return min(allowed_upper, max(allowed_lower, (lower + upper) * 0.5)) @staticmethod def _coverage(points: FloatArray, center: FloatArray, size: FloatArray, yaw: float) -> float: forward = np.asarray([math.cos(yaw), math.sin(yaw)]) side = np.asarray([-math.sin(yaw), math.cos(yaw)]) relative = points[:, :2] - center[:2] local = np.column_stack((relative @ forward, relative @ side)) horizontal = np.all(np.abs(local) <= size[:2] * 0.5 + 1e-6, axis=1) vertical = np.abs(points[:, 2] - center[2]) <= size[2] * 0.5 + 1e-6 return float(np.mean(horizontal & vertical)) def _prune(self, now: float) -> None: maximum_idle = float(self.profile["temporal"]["maximum_idle_s"]) stale = [ key for key, state in self.states.items() if now - state.last_seconds > maximum_idle ] for key in stale: del self.states[key] def fuse_tracks( *, tracks: list[dict[str, Any]], semantic_map: npt.NDArray[np.uint8], pixels: FloatArray, depths: FloatArray, source_indices: npt.NDArray[np.int64], points_map: FloatArray, points_lidar: FloatArray, association: Mapping[str, Any], distance_history: dict[int, deque[float]], completion_tracker: CuboidCompletionTracker | None = None, sensor_position_map: Sequence[float] | None = None, session_seconds: float | None = None, ) -> tuple[TrackFusion, ...]: vehicle_labels = set(str(value) for value in association["vehicle_labels"]) ground_filter = association.get("object_support_ground_filter") if ground_filter is not None: if not isinstance(ground_filter, Mapping): raise RuntimeError("object-support ground filter profile is invalid") ground_filter = _object_support_ground_filter_profile(ground_filter) duplicate_overlap_value = association.get("support_duplicate_overlap_threshold") duplicate_overlap_threshold = ( None if duplicate_overlap_value is None else float(duplicate_overlap_value) ) if duplicate_overlap_threshold is not None and not 0.0 < duplicate_overlap_threshold <= 1.0: raise RuntimeError("support duplicate overlap threshold is invalid") accepted_tracks: list[dict[str, Any]] = [] for track in sorted(tracks, key=lambda value: float(value["score"]), reverse=True): label = str(track["label"]) box = tuple(float(value) for value in track["bbox_xyxy"]) if label in vehicle_labels and any( str(other["label"]) in vehicle_labels and _box_iou(box, other["bbox_xyxy"]) > float(association["group_nms_iou_threshold"]) for other in accepted_tracks ): continue accepted_tracks.append(track) accepted_tracks.sort(key=lambda value: int(value["track_id"])) pixel_x = np.clip(np.rint(pixels[:, 0]).astype(np.int64), 0, IMAGE_WIDTH - 1) pixel_y = np.clip(np.rint(pixels[:, 1]).astype(np.int64), 0, IMAGE_HEIGHT - 1) sampled_semantic = semantic_map[pixel_y, pixel_x] result: list[TrackFusion] = [] for track in accepted_tracks: label = str(track["label"]) group = "vehicle" if label in vehicle_labels else label if group not in association["semantic_ids"]: continue track_id = int(track["track_id"]) x1, y1, x2, y2 = (float(value) for value in track["bbox_xyxy"]) width, height = x2 - x1, y2 - y1 inset = association["box_inset"] inside = ( (pixels[:, 0] >= x1 + width * float(inset["horizontal_fraction"])) & (pixels[:, 0] < x2 - width * float(inset["horizontal_fraction"])) & (pixels[:, 1] >= y1 + height * float(inset["top_fraction"])) & (pixels[:, 1] < y2 - height * float(inset["bottom_fraction"])) ) candidates = np.flatnonzero(inside).astype(np.int64) allowed = np.asarray(association["semantic_ids"][group], dtype=np.uint8) compatible = candidates[np.isin(sampled_semantic[candidates], allowed)] clustered = _depth_cluster(compatible, depths, association) selected_before_ground = _spatial_cluster( source_indices[clustered], points_lidar, float(association["spatial_cluster_radius_m"][group]), ) selected = selected_before_ground support_ground_z = None ground_rejected_points = 0 if ground_filter is not None: selected, support_ground_z, ground_rejected_points = _filter_object_support_by_ground( selected_before_ground, points_map, group=group, profile=ground_filter, ) ranges = np.linalg.norm(points_lidar[selected], axis=1) p10 = None if ranges.size == 0 else float(np.percentile(ranges, 10)) median = None if ranges.size == 0 else float(np.median(ranges)) minimum = int(association["minimum_support_points"][group]) cuboid = None observed_cuboid = None geometry = "none" completion_fraction = None ground_z_map = None orientation_source = None temporal_status = None support_coverage_fraction = None if compatible.size == 0: status = "rejected-no-semantic-lidar-support" elif ( selected_before_ground.size >= minimum and selected.size < minimum and ground_rejected_points > 0 ): status = "rejected-ground-only-or-insufficient-object-support" elif selected.size < minimum: status = f"rejected-fewer-than-{minimum}-clustered-points" else: cuboid = _cuboid(points_map[selected], association, group) status = ( "accepted-point-supported-oriented-p05-p95" if cuboid else "rejected-implausible-cuboid" ) if cuboid is not None: observed_cuboid = cuboid geometry = "point-supported-visible-surface-envelope" smoothed = None if cuboid is not None and median is not None: history = distance_history[track_id] if history: reference = float(np.median(np.asarray(history, dtype=np.float64))) maximum = max( float(association["maximum_distance_innovation_m"]), reference * float(association["maximum_distance_innovation_fraction"]), ) if abs(median - reference) > maximum: cuboid = None status = "rejected-distance-innovation" if cuboid is not None: history.append(median) smoothed = float(np.median(np.asarray(history, dtype=np.float64))) if cuboid is None: observed_cuboid = None geometry = "none" elif completion_tracker is not None: if sensor_position_map is None or session_seconds is None: raise RuntimeError("LAB E13 completion requires sensor pose and session time") completion = completion_tracker.complete( track_id=track_id, label=label, support_points_map=points_map[selected], all_points_map=points_map, sensor_position_map=sensor_position_map, session_seconds=session_seconds, observed_cuboid=observed_cuboid, ) if completion is None: reason = completion_tracker.last_failure_reason or "unknown" if completion_tracker.profile["failure_policy"] == "reject": cuboid = None geometry = "none" status = f"rejected-amodal-completion-{reason}" else: status = f"accepted-visible-envelope-completion-{reason}" else: cuboid = completion.cuboid status = "accepted-class-prior-amodal-v1" geometry = "class-prior-completed-from-visible-lidar-support" completion_fraction = completion.completion_fraction ground_z_map = completion.ground_z_map orientation_source = completion.orientation_source temporal_status = completion.temporal_status support_coverage_fraction = completion.support_coverage_fraction result.append( TrackFusion( track_id=track_id, label=label, association_group=group, score=float(track["score"]), bbox_xyxy=(x1, y1, x2, y2), candidate_points=int(candidates.size), semantic_points=int(compatible.size), clustered_points=int(selected.size), distance_p10_m=p10, distance_median_m=median, distance_smoothed_m=smoothed, status=status, cuboid=cuboid, source_indices=selected, pre_ground_clustered_points=int(selected_before_ground.size), ground_rejected_points=ground_rejected_points, support_ground_z_map=support_ground_z, geometry=geometry, observed_cuboid=observed_cuboid, completion_fraction=completion_fraction, ground_z_map=ground_z_map, orientation_source=orientation_source, temporal_status=temporal_status, support_coverage_fraction=support_coverage_fraction, ) ) if duplicate_overlap_threshold is None: return tuple(result) return _suppress_duplicate_support_fusions( result, overlap_threshold=duplicate_overlap_threshold ) def fusion_document(item: TrackFusion) -> dict[str, Any]: return { "track_id": item.track_id, "label": item.label, "association_group": item.association_group, "score": item.score, "bbox_xyxy": list(item.bbox_xyxy), "candidate_projected_points": item.candidate_points, "semantic_compatible_points": item.semantic_points, "clustered_points": item.clustered_points, "pre_ground_clustered_points": item.pre_ground_clustered_points, "ground_rejected_points": item.ground_rejected_points, "support_ground_z_map": item.support_ground_z_map, "distance_p10_m": item.distance_p10_m, "distance_median_m": item.distance_median_m, "distance_smoothed_m": item.distance_smoothed_m, "cuboid_status": item.status, "cuboid_center_map": None if item.cuboid is None else list(item.cuboid.center_map), "cuboid_half_size": None if item.cuboid is None else list(item.cuboid.half_size), "cuboid_quaternion_xyzw": None if item.cuboid is None else list(item.cuboid.quaternion_xyzw), "geometry": item.geometry, "observed_cuboid_center_map": None if item.observed_cuboid is None else list(item.observed_cuboid.center_map), "observed_cuboid_half_size": None if item.observed_cuboid is None else list(item.observed_cuboid.half_size), "observed_cuboid_quaternion_xyzw": None if item.observed_cuboid is None else list(item.observed_cuboid.quaternion_xyzw), "completion_fraction": item.completion_fraction, "ground_z_map": item.ground_z_map, "orientation_source": item.orientation_source, "temporal_status": item.temporal_status, "support_coverage_fraction": item.support_coverage_fraction, } def clearance(points_lidar: FloatArray, profile: Mapping[str, Any]) -> dict[str, Any]: sectors = int(profile["sector_count"]) empty = { "schema": "diagnostic-polar-obstacle-clearance/v1", "state": "unavailable", "frame": "k1-lidar", "front_m": None, "observed_sector_fraction": 0.0, "sector_ranges_m": [None] * sectors, } if points_lidar.size == 0: return empty ground = float(np.percentile(points_lidar[:, 2], float(profile["ground_percentile"]))) ranges = np.linalg.norm(points_lidar[:, :2], axis=1) keep = ( (ranges >= float(profile["minimum_range_m"])) & (ranges <= float(profile["maximum_range_m"])) & (points_lidar[:, 2] >= ground + float(profile["minimum_height_above_ground_m"])) & (points_lidar[:, 2] <= ground + float(profile["maximum_height_above_ground_m"])) ) points, ranges = points_lidar[keep], ranges[keep] if points.size == 0: return {**empty, "ground_z_estimate_m": ground} angles = np.arctan2(points[:, 1], points[:, 0]) indices = np.floor((angles + math.pi) / (2 * math.pi) * sectors).astype(int) indices = np.clip(indices, 0, sectors - 1) values = np.full(sectors, np.inf, dtype=np.float64) np.minimum.at(values, indices, ranges) observed = np.isfinite(values) front = ranges[np.abs(angles) <= math.radians(float(profile["front_half_angle_degrees"]))] return { "schema": empty["schema"], "state": "available", "frame": "k1-lidar", "front_m": None if front.size == 0 else float(np.min(front)), "observed_sector_fraction": float(np.mean(observed)), "sector_ranges_m": [None if not math.isfinite(value) else float(value) for value in values], "ground_z_estimate_m": ground, } class WorldStateProjector: def __init__(self, history_limit_s: float) -> None: self.history_limit_s = history_limit_s self.history: dict[int, deque[tuple[float, tuple[float, float, float]]]] = {} def project( self, *, frame_index: int, source_frame_index: int, session_seconds: float, fusion_state: str, fusions: Sequence[TrackFusion], points_lidar: FloatArray, clearance_state: Mapping[str, Any], delivery: Mapping[str, Any], ) -> dict[str, Any]: objects: list[dict[str, Any]] = [] for item in fusions: if item.cuboid is None or item.distance_smoothed_m is None: continue center = item.cuboid.center_map velocity, velocity_status, residual = self._velocity( item.track_id, session_seconds, center ) lidar_center = None if item.source_indices.size: lidar_center = np.median(points_lidar[item.source_indices], axis=0) objects.append( { "track_id": item.track_id, "class": item.association_group, "detector_label": item.label, "confidence": item.score, "position_map_m": list(center), "position_lidar_m": None if lidar_center is None else lidar_center.tolist(), "orientation_map_xyzw": list(item.cuboid.quaternion_xyzw), "size_m": [2 * value for value in item.cuboid.half_size], "range_m": item.distance_smoothed_m, "velocity_map_mps": None if velocity is None else list(velocity), "speed_mps": None if velocity is None else math.sqrt(sum(value * value for value in velocity)), "velocity_status": velocity_status, "velocity_residual_m": residual, "support_points": item.clustered_points, "geometry": item.geometry, "completion_fraction": item.completion_fraction, "ground_z_map": item.ground_z_map, "orientation_source": item.orientation_source, "temporal_status": item.temporal_status, "support_coverage_fraction": item.support_coverage_fraction, } ) self._prune(session_seconds) return { "schema_version": WORLD_STATE_SCHEMA, "frame_index": frame_index, "source_frame_index": source_frame_index, "session_seconds": session_seconds, "coordinate_frames": { "world": "k1-map", "sensor_relative": "k1-lidar", "vehicle_body": "unavailable-no-rig-to-vehicle-transform", }, "fusion_state": fusion_state, "delivery": dict(delivery), "objects": objects, "object_count": len(objects), "clearance": dict(clearance_state), } def _velocity( self, track_id: int, now: float, center: tuple[float, float, float] ) -> tuple[tuple[float, float, float] | None, str, float | None]: history = self.history.setdefault(track_id, deque(maxlen=32)) history.append((now, center)) while history and history[0][0] < now - self.history_limit_s: history.popleft() if len(history) < 4 or history[-1][0] - history[0][0] < 0.4: return None, "unavailable-insufficient-history", None slopes = [] values = list(history) for left, (left_time, left_center) in enumerate(values): for right_time, right_center in values[left + 1 :]: delta = right_time - left_time if delta >= 0.2: slopes.append( tuple( (right_center[index] - left_center[index]) / delta for index in range(3) ) ) if not slopes: return None, "unavailable-insufficient-baseline", None velocity = tuple(statistics.median(value[index] for value in slopes) for index in range(3)) latest_time, latest_center = values[-1] residuals = [] for observed_time, observed in values: predicted = tuple( latest_center[index] - velocity[index] * (latest_time - observed_time) for index in range(3) ) residuals.append( math.sqrt(sum((observed[index] - predicted[index]) ** 2 for index in range(3))) ) residual = statistics.median(residuals) speed = math.sqrt(sum(value * value for value in velocity)) if speed > 20: return None, "rejected-speed-bound", residual if residual > 0.75: return None, "rejected-position-residual", residual return velocity, "diagnostic-robust-history", residual def _prune(self, now: float) -> None: expired = [ key for key, value in self.history.items() if not value or value[-1][0] < now - self.history_limit_s ] for key in expired: del self.history[key] def distance_history(length: int) -> dict[int, deque[float]]: return defaultdict(lambda: deque(maxlen=length))