optimize radar point test

selfdrive/ui/onroad/model_renderer.py

@@ -1,5 +1,4 @@
 import colorsys
-import math
 import numpy as np
 import pyray as rl
 from cereal import messaging, car
@@ -8,6 +7,7 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.constants import CV
 from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
 from openpilot.selfdrive.ui.lib.starpilot_theme import get_param_color, get_theme_color, get_visual_color, is_stock_color_scheme, with_alpha
+from openpilot.selfdrive.ui.onroad.radar_tracks import project_radar_points
 from openpilot.selfdrive.ui.onroad.starpilot.rainbow_path import RainbowPath
 from openpilot.selfdrive.ui.lib.starpilot_visuals import lead_indicator_enabled
 from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
@@ -24,6 +24,13 @@ DEFAULT_LANE_LINES_WIDTH = 4.0
 DEFAULT_PATH_EDGE_WIDTH = 20.0
 DEFAULT_PATH_WIDTH = 6.1
 DEFAULT_ROAD_EDGES_WIDTH = 2.0
+RADAR_MARKER_RADIUS = 7.0
+RADAR_MARKER_OUTLINE_RADIUS = 9.0
+RADAR_MARKER_TEXTURE_SIZE = 22
+RADAR_MARKER_TEXTURE_CENTER = RADAR_MARKER_TEXTURE_SIZE / 2.0
+RADAR_MARKER_TEXTURE_KEY = "onroad-radar-marker-v1"
+RADAR_MARKER_OUTLINE_COLOR = rl.Color(0, 0, 0, 170)
+RADAR_MARKER_FILL_COLOR = rl.Color(255, 40, 40, 230)
 
 THROTTLE_COLORS = [
   rl.Color(13, 248, 122, 102),   # HSLF(148/360, 0.94, 0.51, 0.4)
@@ -77,6 +84,12 @@ class ModelRenderer(Widget):
     # Transform matrix (3x3 for car space to screen space)
     self._car_space_transform = np.zeros((3, 3), dtype=np.float32)
     self._transform_dirty = True
+    self._radar_transform_generation = 0
+    self._radar_path_generation = 0
+    self._radar_projection_key = None
+    self._radar_marker_centers = []
+    self._radar_marker_positions = []
+    self._radar_marker_texture = None
     self._clip_region = None
 
     self._exp_gradient = Gradient(
@@ -96,6 +109,7 @@ class ModelRenderer(Widget):
   def set_transform(self, transform: np.ndarray):
     self._car_space_transform = transform.astype(np.float32)
     self._transform_dirty = True
+    self._radar_transform_generation += 1
 
   def _render(self, rect: rl.Rectangle):
     sm = ui_state.sm
@@ -172,6 +186,7 @@ class ModelRenderer(Widget):
   def _update_raw_points(self, model):
     """Update raw 3D points from model data"""
     self._path.raw_points = np.array([model.position.x, model.position.y, model.position.z], dtype=np.float32).T
+    self._radar_path_generation += 1
 
     # Model outputs can vary by branch/model family; keep renderer bounded to
     # the fixed number of lane/edge slots used by the UI.
@@ -591,75 +606,99 @@ class ModelRenderer(Widget):
   def _draw_radar_tracks(self):
     radar_tracks_enabled = self._params.get_bool("RadarTracksUI")
     if not radar_tracks_enabled:
+      self._clear_radar_projection_cache()
       return
 
     sm = ui_state.sm
     if not sm.valid.get("liveTracks", False):
+      self._clear_radar_projection_cache()
       return
 
     radar_points = sm["liveTracks"].points
     if len(radar_points) == 0:
+      self._clear_radar_projection_cache()
       return
 
-    path_x_array = self._path.raw_points[:, 0]
-    line_z = self._path.raw_points[:, 2]
-
-    # Keep raw radar detections visible over bright road imagery without
-    # making them look like confirmed lead-vehicle markers.
-    radius = 7.0
-    outline_radius = 9.0
-    outline_color = rl.Color(0, 0, 0, 170)
-    red_color = rl.Color(255, 40, 40, 230)
-
-    # Pre-extract matrix values and clip bounds for native loop speed
-    t = self._car_space_transform
-    m00, m01, m02 = float(t[0, 0]), float(t[0, 1]), float(t[0, 2])
-    m10, m11, m12 = float(t[1, 0]), float(t[1, 1]), float(t[1, 2])
-    m20, m21, m22 = float(t[2, 0]), float(t[2, 1]), float(t[2, 2])
     clip = self._clip_region
-    clip_x, clip_y = float(clip.x), float(clip.y)
-    clip_xmax, clip_ymax = clip_x + float(clip.width), clip_y + float(clip.height)
-    offset_z = float(self._path_offset_z)
+    if clip is None:
+      self._clear_radar_projection_cache()
+      return
 
-    rect_x, rect_y = float(self._rect.x), float(self._rect.y)
-    rect_xmax, rect_ymax = rect_x + float(self._rect.width), rect_y + float(self._rect.height)
+    projection_key = (
+      sm.recv_frame["liveTracks"],
+      self._radar_path_generation,
+      self._radar_transform_generation,
+      float(self._path_offset_z),
+      float(self._rect.x),
+      float(self._rect.y),
+      float(self._rect.width),
+      float(self._rect.height),
+    )
+    if projection_key != self._radar_projection_key:
+      d_rel = np.fromiter((float(point.dRel) for point in radar_points), dtype=np.float64, count=len(radar_points))
+      in_y = np.fromiter((float(-point.yRel) for point in radar_points), dtype=np.float64, count=len(radar_points))
+      clip_bounds = (
+        float(clip.x),
+        float(clip.y),
+        float(clip.x + clip.width),
+        float(clip.y + clip.height),
+      )
+      rect_bounds = (
+        float(self._rect.x),
+        float(self._rect.y),
+        float(self._rect.x + self._rect.width),
+        float(self._rect.y + self._rect.height),
+      )
+      screen_points = project_radar_points(
+        d_rel,
+        in_y,
+        self._path.raw_points[:, 0],
+        self._path.raw_points[:, 2],
+        self._car_space_transform,
+        float(self._path_offset_z),
+        clip_bounds,
+        rect_bounds,
+      )
+      self._radar_marker_centers = [rl.Vector2(float(x), float(y)) for x, y in screen_points]
+      self._radar_marker_positions = [
+        rl.Vector2(float(x - RADAR_MARKER_TEXTURE_CENTER), float(y - RADAR_MARKER_TEXTURE_CENTER))
+        for x, y in screen_points
+      ]
+      self._radar_projection_key = projection_key
 
-    for point in radar_points:
-      d_rel = float(point.dRel)
-      in_y = float(-point.yRel)
+    cache = getattr(gui_app, "cached_render_texture", None)
+    if self._radar_marker_texture is None and cache is not None:
+      self._radar_marker_texture = cache(
+        RADAR_MARKER_TEXTURE_KEY,
+        RADAR_MARKER_TEXTURE_SIZE,
+        RADAR_MARKER_TEXTURE_SIZE,
+        self._draw_radar_marker_texture,
+      )
 
-      # 0. Reject invalid sensor values before projection
-      if not math.isfinite(d_rel) or not math.isfinite(in_y):
-        continue
+    if self._radar_marker_texture is None:
+      for marker in self._radar_marker_centers:
+        rl.draw_circle_v(marker, RADAR_MARKER_OUTLINE_RADIUS, RADAR_MARKER_OUTLINE_COLOR)
+        rl.draw_circle_v(marker, RADAR_MARKER_RADIUS, RADAR_MARKER_FILL_COLOR)
+      return
 
-      # 1. Fast binary search instead of np.where boolean mask
-      idx = np.searchsorted(path_x_array, d_rel, side='right') - 1
-      idx = int(idx) if idx >= 0 else 0
-      z = float(line_z[idx]) if idx < len(line_z) else 0.0
+    rl.begin_blend_mode(rl.BlendMode.BLEND_ALPHA_PREMULTIPLY)
+    try:
+      for position in self._radar_marker_positions:
+        rl.draw_texture_v(self._radar_marker_texture, position, rl.WHITE)
+    finally:
+      rl.end_blend_mode()
 
-      # 2. Native unrolled 3x3 matrix multiply (bypasses np.array allocation)
-      in_z = z + offset_z
-      pt_w = m20 * d_rel + m21 * in_y + m22 * in_z
+  def _draw_radar_marker_texture(self):
+    center = rl.Vector2(RADAR_MARKER_TEXTURE_CENTER, RADAR_MARKER_TEXTURE_CENTER)
+    rl.draw_circle_v(center, RADAR_MARKER_OUTLINE_RADIUS, RADAR_MARKER_OUTLINE_COLOR)
+    rl.draw_circle_v(center, RADAR_MARKER_RADIUS, RADAR_MARKER_FILL_COLOR)
 
-      # 3. Match _map_to_screen: skip points at the focal plane.
-      if abs(pt_w) < 1e-6:
-        continue
-
-      # 4. Perspective divide (matches _map_to_screen)
-      x = (m00 * d_rel + m01 * in_y + m02 * in_z) / pt_w
-      y = (m10 * d_rel + m11 * in_y + m12 * in_z) / pt_w
-
-      # 5. Clip region check (matches _map_to_screen)
-      if not (clip_x <= x <= clip_xmax and clip_y <= y <= clip_ymax):
-        continue
-
-      # 6. Screen rect clamping (matches original np.clip on calibrated_point)
-      x = max(rect_x, min(x, rect_xmax))
-      y = max(rect_y, min(y, rect_ymax))
-
-      marker = rl.Vector2(x, y)
-      rl.draw_circle_v(marker, outline_radius, outline_color)
-      rl.draw_circle_v(marker, radius, red_color)
+  def _clear_radar_projection_cache(self):
+    if self._radar_projection_key is None and not self._radar_marker_centers and not self._radar_marker_positions:
+      return
+    self._radar_projection_key = None
+    self._radar_marker_centers = []
+    self._radar_marker_positions = []
 
   def _update_adjacent_paths(self, max_idx: int, max_distance: float):
     """Compute adjacent lane path polygons by averaging lane line pairs."""

selfdrive/ui/onroad/radar_tracks.py

@@ -0,0 +1,62 @@
+import numpy as np
+
+
+def project_radar_points(
+  d_rel: np.ndarray,
+  in_y: np.ndarray,
+  path_x: np.ndarray,
+  path_z: np.ndarray,
+  transform: np.ndarray,
+  path_offset_z: float,
+  clip_bounds: tuple[float, float, float, float],
+  rect_bounds: tuple[float, float, float, float],
+) -> np.ndarray:
+  """Project radar points into screen space, preserving source order."""
+  if d_rel.size == 0 or path_x.size == 0:
+    return np.empty((0, 2), dtype=np.float64)
+
+  finite = np.isfinite(d_rel) & np.isfinite(in_y)
+  if not np.any(finite):
+    return np.empty((0, 2), dtype=np.float64)
+
+  d_rel = d_rel[finite]
+  in_y = in_y[finite]
+
+  path_indices = np.searchsorted(path_x, d_rel, side="right") - 1
+  path_indices = np.maximum(path_indices, 0)
+
+  line_z = np.zeros(d_rel.shape, dtype=np.float64)
+  valid_z = path_indices < path_z.size
+  if np.any(valid_z):
+    line_z[valid_z] = path_z[path_indices[valid_z]]
+
+  in_z = line_z + float(path_offset_z)
+
+  # The former scalar implementation converts float32 matrix values to Python
+  # floats before arithmetic. Keep this path in float64 to preserve its
+  # screen-coordinate and boundary behavior.
+  t = transform.astype(np.float64, copy=False)
+  point_w = t[2, 0] * d_rel + t[2, 1] * in_y + t[2, 2] * in_z
+  valid_w = np.abs(point_w) >= 1e-6
+
+  x = np.zeros_like(d_rel)
+  y = np.zeros_like(d_rel)
+  x_num = t[0, 0] * d_rel + t[0, 1] * in_y + t[0, 2] * in_z
+  y_num = t[1, 0] * d_rel + t[1, 1] * in_y + t[1, 2] * in_z
+  np.divide(x_num, point_w, out=x, where=valid_w)
+  np.divide(y_num, point_w, out=y, where=valid_w)
+
+  clip_x, clip_y, clip_xmax, clip_ymax = clip_bounds
+  visible = (
+    valid_w
+    & (x >= clip_x) & (x <= clip_xmax)
+    & (y >= clip_y) & (y <= clip_ymax)
+  )
+  if not np.any(visible):
+    return np.empty((0, 2), dtype=np.float64)
+
+  rect_x, rect_y, rect_xmax, rect_ymax = rect_bounds
+  return np.column_stack((
+    np.clip(x[visible], rect_x, rect_xmax),
+    np.clip(y[visible], rect_y, rect_ymax),
+  ))

selfdrive/ui/tests/test_radar_tracks.py

@@ -0,0 +1,142 @@
+import math
+
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.ui.onroad.radar_tracks import project_radar_points
+
+
+def _scalar_reference(d_rel, in_y, path_x, path_z, transform, path_offset_z, clip_bounds, rect_bounds):
+  clip_x, clip_y, clip_xmax, clip_ymax = clip_bounds
+  rect_x, rect_y, rect_xmax, rect_ymax = rect_bounds
+  result = []
+
+  for d, y in zip(d_rel, in_y, strict=True):
+    d = float(d)
+    y = float(y)
+    if not math.isfinite(d) or not math.isfinite(y):
+      continue
+
+    idx = np.searchsorted(path_x, d, side="right") - 1
+    idx = int(idx) if idx >= 0 else 0
+    z = float(path_z[idx]) if idx < len(path_z) else 0.0
+    in_z = z + float(path_offset_z)
+
+    point_w = (
+      float(transform[2, 0]) * d
+      + float(transform[2, 1]) * y
+      + float(transform[2, 2]) * in_z
+    )
+    if abs(point_w) < 1e-6:
+      continue
+
+    x = (
+      float(transform[0, 0]) * d
+      + float(transform[0, 1]) * y
+      + float(transform[0, 2]) * in_z
+    ) / point_w
+    screen_y = (
+      float(transform[1, 0]) * d
+      + float(transform[1, 1]) * y
+      + float(transform[1, 2]) * in_z
+    ) / point_w
+
+    if not (clip_x <= x <= clip_xmax and clip_y <= screen_y <= clip_ymax):
+      continue
+
+    result.append((
+      max(rect_x, min(x, rect_xmax)),
+      max(rect_y, min(screen_y, rect_ymax)),
+    ))
+
+  return np.asarray(result, dtype=np.float64).reshape(-1, 2)
+
+
+def _projection_inputs(count=65):
+  d_rel = np.asarray([4.0 + i * 2.4 for i in range(count)], dtype=np.float64)
+  in_y = np.asarray([((i % 9) - 4) * 0.45 for i in range(count)], dtype=np.float64)
+  path_x = np.linspace(0.0, 192.0, 33, dtype=np.float32)
+  path_z = (0.15 * np.sin(path_x / 30.0)).astype(np.float32)
+  transform = np.asarray([
+    [18.0, 0.25, 960.0],
+    [0.1, -16.0, 820.0],
+    [0.045, 0.001, 1.0],
+  ], dtype=np.float32)
+  clip_bounds = (-500.0, -500.0, 2420.0, 1580.0)
+  rect_bounds = (0.0, 0.0, 1920.0, 1080.0)
+  return d_rel, in_y, path_x, path_z, transform, clip_bounds, rect_bounds
+
+
+@pytest.mark.parametrize("count", [0, 1, 10, 20, 65])
+def test_vectorized_projection_matches_scalar_reference(count):
+  d_rel, in_y, path_x, path_z, transform, clip_bounds, rect_bounds = _projection_inputs(count)
+  expected = _scalar_reference(d_rel, in_y, path_x, path_z, transform, 1.22, clip_bounds, rect_bounds)
+
+  actual = project_radar_points(
+    d_rel, in_y, path_x, path_z, transform, 1.22, clip_bounds, rect_bounds,
+  )
+
+  assert actual.shape == expected.shape
+  np.testing.assert_allclose(actual, expected, rtol=0.0, atol=1e-10)
+
+
+def test_vectorized_projection_preserves_order_duplicates_and_invalid_values():
+  _, _, path_x, path_z, transform, clip_bounds, rect_bounds = _projection_inputs(0)
+  d_rel = np.asarray([20.0, np.nan, 8.0, 20.0, np.inf, 12.0], dtype=np.float64)
+  in_y = np.asarray([1.0, 2.0, -1.0, 1.0, 3.0, -2.0], dtype=np.float64)
+
+  expected = _scalar_reference(d_rel, in_y, path_x, path_z, transform, 1.22, clip_bounds, rect_bounds)
+  actual = project_radar_points(
+    d_rel, in_y, path_x, path_z, transform, 1.22, clip_bounds, rect_bounds,
+  )
+
+  np.testing.assert_allclose(actual, expected, rtol=0.0, atol=1e-10)
+  assert actual.shape[0] == 4
+  np.testing.assert_allclose(actual[0], actual[2], rtol=0.0, atol=1e-10)
+
+
+def test_vectorized_projection_clips_and_clamps_like_scalar_reference():
+  d_rel, in_y, path_x, path_z, transform, _, _ = _projection_inputs(3)
+  clip_bounds = (0.0, 0.0, 1000.0, 1000.0)
+  rect_bounds = (100.0, 200.0, 900.0, 800.0)
+
+  expected = _scalar_reference(d_rel, in_y, path_x, path_z, transform, 1.22, clip_bounds, rect_bounds)
+  actual = project_radar_points(
+    d_rel, in_y, path_x, path_z, transform, 1.22, clip_bounds, rect_bounds,
+  )
+
+  np.testing.assert_allclose(actual, expected, rtol=0.0, atol=1e-10)
+  assert np.all(actual[:, 0] >= rect_bounds[0])
+  assert np.all(actual[:, 0] <= rect_bounds[2])
+  assert np.all(actual[:, 1] >= rect_bounds[1])
+  assert np.all(actual[:, 1] <= rect_bounds[3])
+
+
+def test_vectorized_projection_handles_path_edges_and_points_beyond_path():
+  d_rel = np.asarray([-5.0, 0.0, 5.0, 25.0], dtype=np.float64)
+  in_y = np.asarray([0.0, 0.0, 0.0, 0.0], dtype=np.float64)
+  path_x = np.asarray([0.0, 10.0], dtype=np.float32)
+  path_z = np.asarray([1.0, 2.0], dtype=np.float32)
+  transform = np.eye(3, dtype=np.float32)
+  clip_bounds = (-100.0, -100.0, 100.0, 100.0)
+  rect_bounds = (-10.0, -10.0, 10.0, 10.0)
+
+  expected = _scalar_reference(d_rel, in_y, path_x, path_z, transform, 1.0, clip_bounds, rect_bounds)
+  actual = project_radar_points(
+    d_rel, in_y, path_x, path_z, transform, 1.0, clip_bounds, rect_bounds,
+  )
+
+  np.testing.assert_allclose(actual, expected, rtol=0.0, atol=1e-10)
+
+
+def test_vectorized_projection_rejects_focal_plane_points():
+  d_rel = np.asarray([1.0, 2.0], dtype=np.float64)
+  in_y = np.asarray([0.0, 0.0], dtype=np.float64)
+  path_x = np.asarray([0.0, 10.0], dtype=np.float32)
+  path_z = np.zeros(2, dtype=np.float32)
+  transform = np.eye(3, dtype=np.float32)
+  bounds = (-100.0, -100.0, 100.0, 100.0)
+
+  actual = project_radar_points(d_rel, in_y, path_x, path_z, transform, 0.0, bounds, bounds)
+
+  assert actual.shape == (0, 2)