diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index e8336727a..a9de47377 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -464,12 +464,12 @@ class ModelRenderer(Widget): rl.draw_triangle_fan(lead.chevron, len(lead.chevron), with_alpha(lead_color, lead.fill_alpha)) @staticmethod - def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int: + def _get_path_length_idx(pos_x_array: np.ndarray, path_distance: float) -> int: """Get the index corresponding to the given path height""" if len(pos_x_array) == 0: return 0 - indices = np.where(pos_x_array <= path_height)[0] - return indices[-1] if indices.size > 0 else 0 + idx = np.searchsorted(pos_x_array, path_distance, side='right') - 1 + return idx if idx >= 0 else 0 def _map_to_screen(self, in_x, in_y, in_z): """Project a point in car space to screen space""" diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index b53f2cad5..3c53b3053 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -1,4 +1,5 @@ import colorsys +import math import numpy as np import pyray as rl from cereal import messaging, car @@ -606,17 +607,53 @@ class ModelRenderer(Widget): radius = 4.0 red_color = rl.Color(255, 0, 0, 200) - for point in radar_points: - d_rel = point.dRel - idx = self._get_path_length_idx(path_x_array, d_rel) - z = line_z[idx] if idx < len(line_z) else 0.0 + # 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) - calibrated_point = self._map_to_screen(d_rel, -point.yRel, z + self._path_offset_z) - if calibrated_point: - x, y = calibrated_point - x = np.clip(x, self._rect.x, self._rect.x + self._rect.width) - y = np.clip(y, self._rect.y, self._rect.y + self._rect.height) - rl.draw_circle_v(rl.Vector2(x, y), radius, red_color) + 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) + + for point in radar_points: + d_rel = float(point.dRel) + in_y = float(-point.yRel) + + # 0. Reject invalid sensor values before projection + if not math.isfinite(d_rel) or not math.isfinite(in_y): + continue + + # 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 + + # 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 + + # 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)) + + rl.draw_circle_v(rl.Vector2(x, y), radius, red_color) def _update_adjacent_paths(self, max_idx: int, max_distance: float): """Compute adjacent lane path polygons by averaging lane line pairs.""" @@ -705,8 +742,8 @@ class ModelRenderer(Widget): """Get the index corresponding to the given path distance""" if len(pos_x_array) == 0: return 0 - indices = np.where(pos_x_array <= path_distance)[0] - return indices[-1] if indices.size > 0 else 0 + idx = np.searchsorted(pos_x_array, path_distance, side='right') - 1 + return idx if idx >= 0 else 0 def _map_to_screen(self, in_x, in_y, in_z): """Project a point in car space to screen space"""