feat(perception): run PointPillars on RAVNOVES00

2026-07-31 14:35:34 +03:00
parent b44c4b3265
commit 55b7821a5c
11 changed files with 1760 additions and 9 deletions
@@ -0,0 +1,135 @@
+"""Deterministic helpers for the L3.1 PointPillars transfer on RAVNOVES00."""
+
+from __future__ import annotations
+
+import math
+from collections.abc import Sequence
+
+import numpy as np
+import numpy.typing as npt
+
+
+class L31PointPillarsRavnovesError(RuntimeError):
+    """The RAVNOVES transfer input violates the frozen L3.1 contract."""
+
+
+def nearest_pose_indices(
+    point_times_ns: npt.NDArray[np.int64],
+    pose_times_ns: npt.NDArray[np.int64],
+) -> tuple[npt.NDArray[np.int64], npt.NDArray[np.float64]]:
+    """Bind each point frame to the nearest pose on the host monotonic clock."""
+
+    point_times = np.asarray(point_times_ns, dtype=np.int64)
+    pose_times = np.asarray(pose_times_ns, dtype=np.int64)
+    if (
+        point_times.ndim != 1
+        or pose_times.ndim != 1
+        or not point_times.size
+        or not pose_times.size
+        or np.any(np.diff(point_times) < 0)
+        or np.any(np.diff(pose_times) < 0)
+    ):
+        raise L31PointPillarsRavnovesError("LiDAR/pose time axes are invalid")
+
+    right = np.searchsorted(pose_times, point_times, side="left")
+    right = np.clip(right, 0, pose_times.size - 1)
+    left = np.clip(right - 1, 0, pose_times.size - 1)
+    right_delta = np.abs(pose_times[right] - point_times)
+    left_delta = np.abs(point_times - pose_times[left])
+    indices = np.where(left_delta <= right_delta, left, right).astype(np.int64)
+    age_ms = (
+        np.abs(pose_times[indices] - point_times).astype(np.float64) / 1_000_000.0
+    )
+    if not np.isfinite(age_ms).all():
+        raise L31PointPillarsRavnovesError("LiDAR/pose binding age is invalid")
+    return indices, age_ms
+
+
+def sensor_frame_xyzi(
+    points_map_xyz: npt.NDArray[np.float64],
+    intensities: npt.NDArray[np.uint8],
+    *,
+    position_map_xyz: npt.NDArray[np.float64],
+    orientation_map_from_lidar_xyzw: npt.NDArray[np.float64],
+) -> npt.NDArray[np.float32]:
+    """Convert verified map-frame K1 points into the model's sensor frame."""
+
+    points = np.asarray(points_map_xyz, dtype=np.float64)
+    intensity = np.asarray(intensities, dtype=np.uint8)
+    position = np.asarray(position_map_xyz, dtype=np.float64)
+    quaternion = np.asarray(orientation_map_from_lidar_xyzw, dtype=np.float64)
+    if (
+        points.ndim != 2
+        or points.shape[1:] != (3,)
+        or intensity.shape != (points.shape[0],)
+        or position.shape != (3,)
+        or quaternion.shape != (4,)
+        or not np.isfinite(points).all()
+        or not np.isfinite(position).all()
+        or not np.isfinite(quaternion).all()
+    ):
+        raise L31PointPillarsRavnovesError("K1 point/pose arrays are invalid")
+    norm = float(np.linalg.norm(quaternion))
+    if not math.isfinite(norm) or not 0.99 <= norm <= 1.01:
+        raise L31PointPillarsRavnovesError("K1 pose quaternion is not normalized")
+    x, y, z, w = quaternion / norm
+    rotation_map_from_lidar = 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,
+    )
+    sensor_xyz = (points - position) @ rotation_map_from_lidar
+    result = np.empty((points.shape[0], 4), dtype=np.float32)
+    result[:, :3] = sensor_xyz.astype(np.float32)
+    result[:, 3] = intensity.astype(np.float32) / 255.0
+    if not np.isfinite(result).all():
+        raise L31PointPillarsRavnovesError("K1 sensor-frame XYZI is non-finite")
+    return result
+
+
+def select_visual_frame_indices(
+    vehicle_counts: Sequence[int],
+    total_counts: Sequence[int],
+    *,
+    maximum_frames: int = 18,
+) -> tuple[int, ...]:
+    """Select route-wide evidence, preferring frames with Vehicle predictions."""
+
+    vehicles = tuple(vehicle_counts)
+    totals = tuple(total_counts)
+    if (
+        len(vehicles) != len(totals)
+        or not vehicles
+        or isinstance(maximum_frames, bool)
+        or maximum_frames < 1
+        or any(
+            isinstance(value, bool) or not isinstance(value, int) or value < 0
+            for value in (*vehicles, *totals)
+        )
+        or any(vehicle > total for vehicle, total in zip(vehicles, totals, strict=True))
+    ):
+        raise L31PointPillarsRavnovesError("L3.1 visual selection input is invalid")
+
+    count = min(maximum_frames, len(vehicles))
+    boundaries = np.linspace(0, len(vehicles), count + 1, dtype=np.int64)
+    selected: list[int] = []
+    for bin_index in range(count):
+        start = int(boundaries[bin_index])
+        stop = int(boundaries[bin_index + 1])
+        if stop <= start:
+            continue
+        center = (start + stop - 1) / 2.0
+        chosen = max(
+            range(start, stop),
+            key=lambda index: (
+                vehicles[index],
+                totals[index],
+                -abs(index - center),
+                -index,
+            ),
+        )
+        selected.append(chosen)
+    return tuple(selected)