diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index c6266a8f9..441042bf6 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/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.""" diff --git a/selfdrive/ui/onroad/radar_tracks.py b/selfdrive/ui/onroad/radar_tracks.py new file mode 100644 index 000000000..f8d0d8e3e --- /dev/null +++ b/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), + )) diff --git a/selfdrive/ui/tests/test_radar_tracks.py b/selfdrive/ui/tests/test_radar_tracks.py new file mode 100644 index 000000000..a644ff8ac --- /dev/null +++ b/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)