diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index ab55f392f75cd7..f1f4e66f5a6809 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -138,9 +138,7 @@ def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = Visio 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 = (0, 0, 0, 0, stream_type) self._cached_matrix: np.ndarray | None = None self._content_rect = rl.Rectangle() self._last_click_time = 0.0 @@ -284,10 +282,19 @@ def _update_calibration(self): 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: + v_ego_quantized = round(ui_state.sm['carState'].vEgo, 1) + cache_key = ( + ui_state.sm.recv_frame['liveCalibration'], + int(self._content_rect.width), + int(self._content_rect.height), + self.stream_type, + v_ego_quantized + ) + + 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 @@ -323,9 +330,7 @@ def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: 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],