diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index 7c0d8ac34..703dcf78b 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -139,9 +139,7 @@ class AugmentedRoadView(CameraView): self.view_from_calib = view_frame_from_device_frame.copy() self.view_from_wide_calib = view_frame_from_device_frame.copy() - self._last_calib_time: float = 0 - self._last_rect_dims = (0.0, 0.0) - self._last_stream_type = stream_type + self._matrix_cache_key: tuple | None = None self._cached_matrix: np.ndarray | None = None self._content_rect = rl.Rectangle() self._last_click_time = 0.0 @@ -295,10 +293,18 @@ class AugmentedRoadView(CameraView): self.view_from_wide_calib = view_frame_from_device_frame @ wide_from_device @ device_from_calib def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: + cache_key = ( + ui_state.sm.recv_frame['liveCalibration'], + int(self._content_rect.width), + int(self._content_rect.height), + self.stream_type, + round(ui_state.sm['carState'].vEgo, 1), + ) + + if cache_key == self._matrix_cache_key and self._cached_matrix is not None: + return self._cached_matrix + # Get camera configuration - # TODO: cache with vEgo? - calib_time = ui_state.sm.recv_frame['liveCalibration'] - current_dims = (self._content_rect.width, self._content_rect.height) device_camera = self.device_camera or DEFAULT_DEVICE_CAMERA is_wide_camera = self.stream_type == WIDE_CAM intrinsic = device_camera.ecam.intrinsics if is_wide_camera else device_camera.fcam.intrinsics @@ -314,7 +320,6 @@ class AugmentedRoadView(CameraView): kep = calib_transform @ inf_point # Calculate center points and dimensions - x, y = self._content_rect.x, self._content_rect.y w, h = self._content_rect.width, self._content_rect.height cx, cy = intrinsic[0, 2], intrinsic[1, 2] @@ -334,18 +339,17 @@ class AugmentedRoadView(CameraView): x_offset, y_offset = 0, 0 # Cache the computed transformation matrix to avoid recalculations - self._last_calib_time = calib_time - self._last_rect_dims = current_dims - self._last_stream_type = self.stream_type + self._matrix_cache_key = cache_key self._cached_matrix = np.array([ [zoom * 2 * cx / w, 0, -x_offset / w * 2], [0, zoom * 2 * cy / h, -y_offset / h * 2], [0, 0, 1.0] ]) + # built without rect.x/y so cache stays hot during scroll. ModelRenderer adds offset at draw time video_transform = np.array([ - [zoom, 0.0, (w / 2 + x - x_offset) - (cx * zoom)], - [0.0, zoom, (h / 2 + y - y_offset) - (cy * zoom)], + [zoom, 0.0, (w / 2 - x_offset) - (cx * zoom)], + [0.0, zoom, (h / 2 - y_offset) - (cy * zoom)], [0.0, 0.0, 1.0] ]) self._model_renderer.set_transform(video_transform @ calib_transform) diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index 3f1badfe8..12500bdcd 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -72,7 +72,7 @@ class ModelRenderer(Widget): self._torque_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps) self._ll_color_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) - # Transform matrix (3x3 for car space to screen space) + # 3x3 car space -> rect-origin space (draw methods add rect.x/y) self._car_space_transform = np.zeros((3, 3), dtype=np.float32) self._transform_dirty = True self._clip_region = None @@ -221,7 +221,8 @@ class ModelRenderer(Widget): if not self._experimental_mode: return - max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x)) + path_pts = self._path.projected_points + np.array([self._rect.x, self._rect.y], dtype=np.float32) + max_len = min(len(path_pts) // 2, len(self._acceleration_x)) segment_colors = [] gradient_stops = [] @@ -229,7 +230,7 @@ class ModelRenderer(Widget): i = 0 while i < max_len: # Some points (screen space) are out of frame (rect space) - track_y = self._path.projected_points[i][1] + track_y = path_pts[i][1] if track_y < self._rect.y or track_y > (self._rect.y + self._rect.height): i += 1 continue @@ -305,14 +306,15 @@ class ModelRenderer(Widget): return color def _draw_lane_lines(self): - """Draw lane lines and road edges""" - """Two closest lines should be green (lane line or road edges)""" + """Draw lane lines and road edges. Two closest lines should be green (lane line or road edges).""" + offset = np.array([self._rect.x, self._rect.y], dtype=np.float32) + for i, lane_line in enumerate(self._lane_lines): if lane_line.projected_points.size == 0: continue color = self._get_ll_color(float(self._lane_line_probs[i]), i in (1, 2), i in (0, 1)) - draw_polygon(self._rect, lane_line.projected_points, color) + draw_polygon(self._rect, lane_line.projected_points + offset, color) for i, road_edge in enumerate(self._road_edges): if road_edge.projected_points.size == 0: @@ -320,7 +322,7 @@ class ModelRenderer(Widget): # if closest lane lines are not confident, make road edges green color = self._get_ll_color(float(1.0 - self._road_edge_stds[i]), float(self._lane_line_probs[i + 1]) < 0.25, i == 0) - draw_polygon(self._rect, road_edge.projected_points, color) + draw_polygon(self._rect, road_edge.projected_points + offset, color) def _draw_path(self, sm): """Draw path with dynamic coloring based on mode and throttle state.""" @@ -330,14 +332,16 @@ class ModelRenderer(Widget): allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control self._blend_filter.update(int(allow_throttle)) + path_pts = self._path.projected_points + np.array([self._rect.x, self._rect.y], dtype=np.float32) + if self._experimental_mode: # Draw with acceleration coloring if ui_state.status == UIStatus.DISENGAGED: - draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90)) + draw_polygon(self._rect, path_pts, rl.Color(0, 0, 0, 90)) elif len(self._exp_gradient.colors) > 1: - draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient) + draw_polygon(self._rect, path_pts, gradient=self._exp_gradient) else: - draw_polygon(self._rect, self._path.projected_points, rl.Color(255, 255, 255, 30)) + draw_polygon(self._rect, path_pts, rl.Color(255, 255, 255, 30)) else: # Blend throttle/no throttle colors based on transition blend_factor = round(self._blend_filter.x * 100) / 100 @@ -350,9 +354,9 @@ class ModelRenderer(Widget): ) if ui_state.status == UIStatus.DISENGAGED: - draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90)) + draw_polygon(self._rect, path_pts, rl.Color(0, 0, 0, 90)) else: - draw_polygon(self._rect, self._path.projected_points, gradient=gradient) + draw_polygon(self._rect, path_pts, gradient=gradient) def _draw_lead_indicator(self): # Draw lead vehicles if available