Add OnePlanner driver + LiDAR pipeline (depends on #9)

src/eval/src/eval/accumulator.py

@@ -33,6 +33,7 @@
     RAABB,
     Cameras,
     DriverResponses,
+    Lidars,
     RenderableTrajectory,
     Routes,
     ScenarioEvalInput,
@@ -88,8 +89,9 @@ class EvalDataAccumulator:
         default_factory=list, init=False
     )
 
-    # Camera and route data
+    # Camera, lidar, and route data
     _cameras: Cameras = field(default_factory=Cameras, init=False)
+    _lidars: Lidars = field(default_factory=Lidars, init=False)
     _routes: Routes = field(default_factory=Routes, init=False)
 
     @property
@@ -120,6 +122,10 @@ def handle_message(self, message: LogEntry) -> None:
             self._handle_actor_poses(message.actor_poses)
         elif msg_type == "driver_camera_image":
             self._cameras.add_camera_image(message.driver_camera_image.camera_image)
+        elif msg_type == "driver_lidar_point_cloud":
+            self._lidars.add_lidar_point_cloud(
+                message.driver_lidar_point_cloud.lidar_point_cloud
+            )
         elif msg_type == "route_request":
             self._routes.add_route(message.route_request.route)
         elif msg_type == "driver_request":
@@ -367,6 +373,7 @@ def build_scenario_eval_input(
             driver_responses=driver_responses,
             vec_map=vec_map,
             cameras=self._cameras if self._cameras.camera_by_logical_id else None,
+            lidars=self._lidars if self._lidars.lidar_by_logical_id else None,
             routes=self._routes if self._routes.routes_in_rig_frame else None,
             run_uuid=run_uuid,
             run_name=run_name,

src/eval/src/eval/data.py

@@ -14,7 +14,12 @@
 import polars as pl
 import shapely
 from alpasim_grpc.v0 import common_pb2
-from alpasim_grpc.v0.egodriver_pb2 import DriveResponse, RolloutCameraImage, Route
+from alpasim_grpc.v0.egodriver_pb2 import (
+    DriveResponse,
+    RolloutCameraImage,
+    RolloutLidarPointCloud,
+    Route,
+)
 from alpasim_grpc.v0.logging_pb2 import RolloutMetadata
 from alpasim_grpc.v0.sensorsim_pb2 import AvailableCamerasReturn, CameraSpec
 from alpasim_utils import geometry
@@ -1463,6 +1468,61 @@ def add_calibration(
         self.calibrations_by_logical_id[calibration.logical_id] = calibration
 
 
+@dataclasses.dataclass
+class Lidar:
+    """A single LiDAR sensor's captured sweeps over time.
+
+    Points are stored in the sensor's rig frame (identity sensor-to-base) at the
+    end-of-spin timestamp reported by the renderer.
+    """
+
+    logical_id: str
+    timestamps_us: list[int] = dataclasses.field(default_factory=list)
+    points_list: list[np.ndarray] = dataclasses.field(default_factory=list)
+
+    def add_point_cloud(
+        self, point_cloud: RolloutLidarPointCloud.LidarPointCloud
+    ) -> None:
+        if point_cloud.point_xyzs_buffer:
+            xyz = np.frombuffer(
+                point_cloud.point_xyzs_buffer, dtype=np.float32
+            ).reshape(-1, 3)
+        else:
+            xyz = np.empty((0, 3), dtype=np.float32)
+        self.timestamps_us.append(point_cloud.frame_end_us)
+        self.points_list.append(xyz)
+
+    def points_at_time(
+        self, time_us: int, max_stale_us: int = 200_000
+    ) -> np.ndarray | None:
+        """Return points from the sweep nearest to `time_us`.
+
+        Returns None when no sweep is within `max_stale_us` of the requested time.
+        """
+        if not self.timestamps_us:
+            return None
+        timestamps = np.asarray(self.timestamps_us, dtype=np.int64)
+        idx = int(np.argmin(np.abs(timestamps - int(time_us))))
+        if abs(int(timestamps[idx]) - int(time_us)) > max_stale_us:
+            return None
+        return self.points_list[idx]
+
+
+@dataclasses.dataclass
+class Lidars:
+    """LiDAR sweeps keyed by logical_id."""
+
+    lidar_by_logical_id: dict[str, Lidar] = dataclasses.field(default_factory=dict)
+
+    def add_lidar_point_cloud(
+        self, point_cloud: RolloutLidarPointCloud.LidarPointCloud
+    ) -> None:
+        logical_id = point_cloud.logical_id
+        if logical_id not in self.lidar_by_logical_id:
+            self.lidar_by_logical_id[logical_id] = Lidar(logical_id=logical_id)
+        self.lidar_by_logical_id[logical_id].add_point_cloud(point_cloud)
+
+
 @dataclasses.dataclass
 class Routes:
     """Captures routes for all timesteps.
@@ -1764,6 +1824,9 @@ class ScenarioEvalInput:
     # Cameras data (optional, needed for image-based metrics)
     cameras: Cameras | None = None
 
+    # LiDAR sweeps captured per sensor (optional, used for video overlays)
+    lidars: Lidars | None = None
+
     # Routes data (optional)
     routes: Routes | None = None
 
@@ -1804,6 +1867,7 @@ class SimulationResult:
     # Shapely polygons and pre-cached STRtrees at each ts for fast spatial queries
     actor_polygons: ActorPolygons
     cameras: Cameras
+    lidars: Lidars
     routes: Routes
     # See ScenarioEvalInput.force_gt_duration_us.
     force_gt_duration_us: int | None = None
@@ -1907,10 +1971,13 @@ def from_scenario_input(
         # Create actor polygons from trajectories
         actor_polygons = ActorPolygons.from_actor_trajectories(actor_trajectories)
 
-        # Create empty cameras and routes if not provided
+        # Create empty cameras, lidars, and routes if not provided
         cameras = (
             scenario_input.cameras if scenario_input.cameras is not None else Cameras()
         )
+        lidars = (
+            scenario_input.lidars if scenario_input.lidars is not None else Lidars()
+        )
         routes = (
             scenario_input.routes if scenario_input.routes is not None else Routes()
         )
@@ -1925,6 +1992,7 @@ def from_scenario_input(
             vec_map=scenario_input.vec_map,
             actor_polygons=actor_polygons,
             cameras=cameras,
+            lidars=lidars,
             routes=routes,
             force_gt_duration_us=scenario_input.force_gt_duration_us,
         )

src/eval/src/eval/schema.py

@@ -77,6 +77,17 @@ class VideoRendererConfig:
     camera_id_to_render: str = MISSING
     # Whether to overlay planner trajectories onto the camera view
     overlay_plans_on_camera: bool = True
+    # Whether to project LiDAR points onto the camera view, colored by depth.
+    overlay_lidar_on_camera: bool = False
+    # LiDAR logical id to overlay when overlay_lidar_on_camera is enabled.
+    # If None and overlay is enabled, the first available lidar is used.
+    lidar_id_to_overlay: str | None = None
+    # Marker size (in points) for projected LiDAR points on the camera view.
+    lidar_overlay_point_size: float = 0.5
+    # Maximum number of LiDAR points to draw per frame (raw sweeps are ~250k
+    # points, which is prohibitively slow for matplotlib scatter). Points are
+    # uniformly subsampled to this cap.
+    lidar_overlay_max_points: int = 8000
     # Order and inclusion of metrics in the rendered table; None shows all metrics.
     metrics_table_entries: list[str] | None = None
     # Options for how to render the BEV map

src/eval/src/eval/video.py

@@ -17,7 +17,7 @@
 
 from eval.aggregation import processing
 from eval.aggregation.processing import ProcessedMetricDFs
-from eval.data import CameraProjector, ScenarioEvalInput, SimulationResult
+from eval.data import CameraProjector, Lidar, ScenarioEvalInput, SimulationResult
 from eval.schema import EvalConfig, MapElements, VideoLayout
 from eval.video_data import ShapelyMap
 from eval.video_reasoning_overlay_utils import render_reasoning_overlay_style_video
@@ -404,6 +404,37 @@ def update_table(
     return table
 
 
+def _render_lidar_overlay(
+    ax: plt.Axes,
+    camera_projector: CameraProjector,
+    lidar: Lidar,
+    time_us: int,
+    point_size: float,
+    max_points: int,
+) -> plt.Artist | None:
+    """Project a LiDAR sweep onto the camera axes, colored by forward depth."""
+    points_rig = lidar.points_at_time(time_us)
+    if points_rig is None or points_rig.size == 0:
+        return None
+    if max_points > 0 and points_rig.shape[0] > max_points:
+        stride = points_rig.shape[0] // max_points
+        points_rig = points_rig[::stride]
+    pixels, mask = camera_projector.project_points(points_rig)
+    if pixels.shape[0] == 0:
+        return None
+    depths = points_rig[mask, 0]
+    return ax.scatter(
+        pixels[:, 0],
+        pixels[:, 1],
+        c=depths,
+        cmap="turbo",
+        s=point_size,
+        vmin=1.0,
+        vmax=60.0,
+        linewidths=0,
+    )
+
+
 def create_video_animation(
     processed_metrics_dfs: ProcessedMetricDFs,
     sim_result: SimulationResult,
@@ -441,35 +472,38 @@ def create_video_animation(
         axs["image"].set_autoscale_on(False)
 
     overlay_enabled = cfg.video.overlay_plans_on_camera
+    lidar_overlay_enabled = cfg.video.overlay_lidar_on_camera
     camera_projector: CameraProjector | None = None
-    if overlay_enabled:
-        if not sim_result.driver_responses.per_timestep_driver_responses:
-            logger.info("No driver responses found; disabling camera overlay.")
+    if overlay_enabled or lidar_overlay_enabled:
+        calibration = sim_result.cameras.calibrations_by_logical_id.get(
+            cfg.video.camera_id_to_render
+        )
+        if calibration is None:
+            logger.warning(
+                "No calibration for camera %s; disabling camera overlays.",
+                cfg.video.camera_id_to_render,
+            )
             overlay_enabled = False
+            lidar_overlay_enabled = False
         else:
-            calibration = sim_result.cameras.calibrations_by_logical_id.get(
-                cfg.video.camera_id_to_render
-            )
-            if calibration is None:
+            try:
+                camera_projector = CameraProjector(
+                    calibration=calibration,
+                    actual_resolution=first_image.size if first_image else None,
+                )
+            except ValueError as exc:
                 logger.warning(
-                    "No calibration for camera %s; disabling camera overlay.",
+                    "Unsupported calibration for camera %s (%s); "
+                    "disabling camera overlays.",
                     cfg.video.camera_id_to_render,
+                    exc,
                 )
                 overlay_enabled = False
-            else:
-                try:
-                    camera_projector = CameraProjector(
-                        calibration=calibration,
-                        actual_resolution=first_image.size if first_image else None,
-                    )
-                except ValueError as exc:
-                    logger.warning(
-                        "Unsupported calibration for camera %s (%s); "
-                        "disabling camera overlay.",
-                        cfg.video.camera_id_to_render,
-                        exc,
-                    )
-                    overlay_enabled = False
+                lidar_overlay_enabled = False
+
+    if overlay_enabled and not sim_result.driver_responses.per_timestep_driver_responses:
+        logger.info("No driver responses found; disabling camera overlay.")
+        overlay_enabled = False
 
     if overlay_enabled:
         overlay_frame_matches = np.intersect1d(
@@ -481,6 +515,27 @@ def create_video_animation(
                 "camera overlay will be empty."
             )
 
+    lidar_overlay_source = None
+    if lidar_overlay_enabled:
+        lidars_by_id = sim_result.lidars.lidar_by_logical_id
+        if not lidars_by_id:
+            logger.info("No LiDAR sweeps recorded; disabling LiDAR overlay.")
+            lidar_overlay_enabled = False
+        else:
+            requested_id = cfg.video.lidar_id_to_overlay
+            if requested_id is None:
+                lidar_overlay_source = next(iter(lidars_by_id.values()))
+            elif requested_id in lidars_by_id:
+                lidar_overlay_source = lidars_by_id[requested_id]
+            else:
+                logger.warning(
+                    "Configured lidar_id_to_overlay=%s not present in recorded "
+                    "sweeps (available=%s); disabling LiDAR overlay.",
+                    requested_id,
+                    list(lidars_by_id.keys()),
+                )
+                lidar_overlay_enabled = False
+
     if should_render_table:
         table = render_table(
             axs["table"],
@@ -700,12 +755,29 @@ def update(time: int) -> list[plt.Artist]:
             command_text_artist.set_visible(False)
 
         overlay_artists: list[plt.Artist] = []
-        if overlay_enabled and camera_projector is not None:
-            overlay_artists = sim_result.driver_responses.render_on_camera(
+        if (
+            lidar_overlay_enabled
+            and camera_projector is not None
+            and lidar_overlay_source is not None
+        ):
+            lidar_artist = _render_lidar_overlay(
                 axs["image"],
                 camera_projector,
+                lidar_overlay_source,
                 time,
-                which_time="now",
+                cfg.video.lidar_overlay_point_size,
+                cfg.video.lidar_overlay_max_points,
+            )
+            if lidar_artist is not None:
+                overlay_artists.append(lidar_artist)
+        if overlay_enabled and camera_projector is not None:
+            overlay_artists.extend(
+                sim_result.driver_responses.render_on_camera(
+                    axs["image"],
+                    camera_projector,
+                    time,
+                    which_time="now",
+                )
             )
             overlay_artists.extend(
                 sim_result.routes.render_on_camera(

src/eval/tests/test_video_reasoning_overlay_utils.py

@@ -20,6 +20,7 @@
     Cameras,
     DriverResponseAtTime,
     DriverResponses,
+    Lidars,
     RenderableTrajectory,
     Routes,
     SimulationResult,
@@ -91,6 +92,7 @@ def test_render_single_reasoning_overlay_frame_uses_renderable_trajectory_api()
         vec_map=None,
         actor_polygons=ActorPolygons.from_actor_trajectories({"EGO": ego_traj}),
         cameras=cameras,
+        lidars=Lidars(),
         routes=Routes(),
     )
     cfg = create_test_eval_config()