diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index 0923bff743..aeaecafb89 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -565,6 +565,8 @@ class StandstillTimerOverlay: class AugmentedRoadView(CameraView): + _use_roi_upload = True + def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD): super().__init__("camerad", stream_type) self._bookmark_callback = bookmark_callback diff --git a/selfdrive/ui/onroad/cameraview.py b/selfdrive/ui/onroad/cameraview.py index ac5c52ffab..bcfa881ff4 100644 --- a/selfdrive/ui/onroad/cameraview.py +++ b/selfdrive/ui/onroad/cameraview.py @@ -1,3 +1,6 @@ +from __future__ import annotations + +import math import os import platform import weakref @@ -147,6 +150,7 @@ FRAME_FRAGMENT_SHADER_YUV_MICI = VERSION + """ class CameraView(Widget): _use_upstream_engaged_color = False + _use_roi_upload = False def __init__(self, name: str, stream_type: VisionStreamType): super().__init__() @@ -183,6 +187,8 @@ class CameraView(Widget): self._regressive_frame_count = 0 self.texture_y: rl.Texture | None = None self.texture_uv: rl.Texture | None = None + self._texture_frame_data: np.ndarray | None = None + self._texture_upload_rect: tuple[int, int, int, int] | None = None # EGL resources self.egl_images: dict[int, EGLImage] = {} @@ -413,7 +419,11 @@ class CameraView(Widget): self._fallback_to_textures("EGL frame rendering failed") if not self._use_egl: - self._render_textures(src_rect, dst_rect) + if (self._use_roi_upload and self.frame.height % 2 == 0 and self.frame.stride % 2 == 0 and + self._stream_type in (VisionStreamType.VISION_STREAM_ROAD, VisionStreamType.VISION_STREAM_WIDE_ROAD)): + self._render_textures(src_rect, dst_rect, rect) + else: + self._render_textures(src_rect, dst_rect) def _draw_placeholder(self, rect: rl.Rectangle): if self._placeholder_color: @@ -516,7 +526,47 @@ class CameraView(Widget): except Exception: cloudlog.exception("CameraView texture fallback initialization failed") - def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: + def _upload_texture_region(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle, clip: rl.Rectangle) -> bool: + frame = self.frame + stride, height = frame.stride, frame.height + if self._texture_needs_update: + # Camera buffers can be reused before a later viewport change exposes more pixels. + size = stride * height * 3 // 2 + if self._texture_frame_data is None or len(self._texture_frame_data) != size: + self._texture_frame_data = np.empty(size, dtype=np.uint8) + self._texture_frame_data[:stride * height] = frame.data[:stride * height] + self._texture_frame_data[stride * height:] = frame.data[frame.uv_offset:frame.uv_offset + stride * (height // 2)] + self._texture_upload_rect = None + self._texture_needs_update = False + + left = max(clip.x, dst_rect.x) + top = max(clip.y, dst_rect.y) + right = min(clip.x + clip.width, dst_rect.x + dst_rect.width) + bottom = min(clip.y + clip.height, dst_rect.y + dst_rect.height) + if left >= right or top >= bottom: + return False + + x0 = max(0, math.floor(src_rect.x + (left - dst_rect.x) * src_rect.width / dst_rect.width) - 2) // 2 * 2 + y0 = max(0, math.floor(src_rect.y + (top - dst_rect.y) * src_rect.height / dst_rect.height) - 2) // 2 * 2 + x1 = min(stride, (math.ceil(src_rect.x + (right - dst_rect.x) * src_rect.width / dst_rect.width) + 3) // 2 * 2) + y1 = min(height, (math.ceil(src_rect.y + (bottom - dst_rect.y) * src_rect.height / dst_rect.height) + 3) // 2 * 2) + if self._texture_upload_rect is not None: + prev_x0, prev_y0, prev_x1, prev_y1 = self._texture_upload_rect + if prev_x0 <= x0 and prev_y0 <= y0 and prev_x1 >= x1 and prev_y1 >= y1: + return True + x0, y0 = min(x0, prev_x0), min(y0, prev_y0) + x1, y1 = max(x1, prev_x1), max(y1, prev_y1) + + y_plane = self._texture_frame_data[:stride * height].reshape(height, stride) + uv_plane = self._texture_frame_data[stride * height:].reshape(height // 2, stride // 2, 2) + y_data = np.ascontiguousarray(y_plane[y0:y1, x0:x1]) + uv_data = np.ascontiguousarray(uv_plane[y0 // 2:y1 // 2, x0 // 2:x1 // 2]) + rl.update_texture_rec(self.texture_y, rl.Rectangle(x0, y0, x1 - x0, y1 - y0), rl.ffi.cast("void *", rl.ffi.from_buffer(y_data))) + rl.update_texture_rec(self.texture_uv, rl.Rectangle(x0 // 2, y0 // 2, (x1 - x0) // 2, (y1 - y0) // 2), rl.ffi.cast("void *", rl.ffi.from_buffer(uv_data))) + self._texture_upload_rect = x0, y0, x1, y1 + return True + + def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle, clip: rl.Rectangle | None = None) -> None: """Copy camera data into ordinary Raylib textures before drawing. Raylib batches camera draws as GL_TEXTURE_2D. Imported EGL images are @@ -529,7 +579,10 @@ class CameraView(Widget): return # Update textures with new frame data - if self._texture_needs_update: + if clip is not None: + if not self._upload_texture_region(src_rect, dst_rect, clip): + return + elif self._texture_needs_update: y_data = self.frame.data[: self.frame.uv_offset] uv_data = self.frame.data[self.frame.uv_offset:] @@ -681,6 +734,8 @@ class CameraView(Widget): cloudlog.exception("CameraView failed to unload temporary EGL image") def _clear_textures(self): + self._texture_frame_data = None + self._texture_upload_rect = None if ((self._external_texture_id or self.egl_texture is not None or self.egl_images) and is_egl_initialized()): try: # Raylib queues draw calls. Submit them before waiting for the GPU so diff --git a/selfdrive/ui/tests/test_camera_roi_upload.py b/selfdrive/ui/tests/test_camera_roi_upload.py new file mode 100644 index 0000000000..d53236aa92 --- /dev/null +++ b/selfdrive/ui/tests/test_camera_roi_upload.py @@ -0,0 +1,143 @@ +from types import SimpleNamespace + +import numpy as np +import pytest +import pyray as rl + +from openpilot.selfdrive.ui.onroad.cameraview import CameraView + + +@pytest.fixture +def camera(monkeypatch): + view = object.__new__(CameraView) + stride, height, uv_offset = 32, 20, 800 + data = np.arange(uv_offset + stride * height // 2, dtype=np.uint8) + view.frame = SimpleNamespace(width=24, height=height, stride=stride, uv_offset=uv_offset, data=data) + view.texture_y = SimpleNamespace(id=1) + view.texture_uv = SimpleNamespace(id=2) + view._texture_needs_update = True + view._texture_frame_data = None + view._texture_upload_rect = None + uploads = [] + + def upload(texture, rect, pointer): + bpp = 1 if texture.id == 1 else 2 + size = int(rect.width * rect.height * bpp) + pixels = np.frombuffer(rl.ffi.buffer(pointer, size), dtype=np.uint8).copy() + uploads.append((texture.id, (int(rect.x), int(rect.y), int(rect.width), int(rect.height)), pixels)) + + monkeypatch.setattr(rl, 'update_texture_rec', upload) + yield view, uploads + view.texture_y = view.texture_uv = None + view._closed = True + + +def test_upload_packs_padded_rows_and_preserves_uv_pairs(camera): + view, uploads = camera + frame = view.frame + assert view._upload_texture_region(rl.Rectangle(0, 0, 24, 20), rl.Rectangle(-24, -20, 72, 60), rl.Rectangle(0, 0, 24, 20)) + + assert len(uploads) == 2 + x0, y0, x1, y1 = view._texture_upload_rect + assert all(value % 2 == 0 for value in (x0, y0, x1, y1)) + assert uploads[0][:2] == (1, (x0, y0, x1 - x0, y1 - y0)) + assert uploads[1][:2] == (2, (x0 // 2, y0 // 2, (x1 - x0) // 2, (y1 - y0) // 2)) + y_plane = frame.data[:frame.stride * frame.height].reshape(frame.height, frame.stride) + uv_plane = frame.data[frame.uv_offset:].reshape(frame.height // 2, frame.stride // 2, 2) + np.testing.assert_array_equal(uploads[0][2], y_plane[y0:y1, x0:x1].ravel()) + np.testing.assert_array_equal(uploads[1][2], uv_plane[y0 // 2:y1 // 2, x0 // 2:x1 // 2].ravel()) + assert not np.shares_memory(view._texture_frame_data, frame.data) + + +def test_unchanged_or_smaller_viewport_does_not_upload_again(camera): + view, uploads = camera + src, dst = rl.Rectangle(0, 0, 24, 20), rl.Rectangle(0, 0, 24, 20) + assert view._upload_texture_region(src, dst, rl.Rectangle(5, 5, 10, 10)) + assert view._upload_texture_region(src, dst, rl.Rectangle(7, 7, 5, 5)) + assert len(uploads) == 2 + + +def test_expanding_viewport_uses_owned_frame_after_camera_buffer_reuse(camera): + view, uploads = camera + src, dst = rl.Rectangle(0, 0, 24, 20), rl.Rectangle(0, 0, 24, 20) + view._upload_texture_region(src, dst, rl.Rectangle(8, 8, 4, 4)) + expected = view.frame.data.copy() + view.frame.data[:] = 255 + + assert view._upload_texture_region(src, dst, rl.Rectangle(0, 0, 24, 20)) + + x0, y0, x1, y1 = view._texture_upload_rect + np.testing.assert_array_equal(uploads[-2][2], expected[:640].reshape(20, 32)[y0:y1, x0:x1].ravel()) + np.testing.assert_array_equal(uploads[-1][2], expected[800:].reshape(10, 16, 2)[y0 // 2:y1 // 2, x0 // 2:x1 // 2].ravel()) + + +def test_new_camera_frame_invalidates_coverage_and_reuses_snapshot_storage(camera): + view, uploads = camera + src, dst = rl.Rectangle(0, 0, 24, 20), rl.Rectangle(0, 0, 24, 20) + view._upload_texture_region(src, dst, rl.Rectangle(0, 0, 24, 20)) + storage = view._texture_frame_data + view.frame.data[:] = 37 + view._texture_needs_update = True + + view._upload_texture_region(src, dst, rl.Rectangle(8, 8, 4, 4)) + + assert view._texture_frame_data is storage + assert view._texture_upload_rect == (6, 6, 14, 14) + np.testing.assert_array_equal(uploads[-2][2], 37) + np.testing.assert_array_equal(uploads[-1][2], 37) + + +def test_invisible_frame_is_owned_before_it_becomes_visible(camera): + view, uploads = camera + src, dst = rl.Rectangle(0, 0, 24, 20), rl.Rectangle(0, 0, 24, 20) + assert not view._upload_texture_region(src, dst, rl.Rectangle(50, 50, 10, 10)) + assert not uploads + expected = view._texture_frame_data.copy() + view.frame.data[:] = 0 + + assert view._upload_texture_region(src, dst, rl.Rectangle(0, 0, 24, 20)) + np.testing.assert_array_equal(view._texture_frame_data, expected) + assert len(uploads) == 2 + + +def test_disjoint_viewports_upload_the_bounding_region_without_holes(camera): + view, uploads = camera + src, dst = rl.Rectangle(0, 0, 24, 20), rl.Rectangle(0, 0, 24, 20) + view._upload_texture_region(src, dst, rl.Rectangle(0, 0, 2, 2)) + view._upload_texture_region(src, dst, rl.Rectangle(22, 18, 2, 2)) + count = len(uploads) + + assert view._upload_texture_region(src, dst, rl.Rectangle(9, 9, 2, 2)) + assert len(uploads) == count + assert view._texture_upload_rect == (0, 0, 26, 20) + + +@pytest.mark.parametrize(('enabled', 'stream', 'stride', 'height', 'region_expected'), [ + (False, 'VISION_STREAM_ROAD', 32, 20, False), + (True, 'VISION_STREAM_ROAD', 32, 20, True), + (True, 'VISION_STREAM_WIDE_ROAD', 32, 20, True), + (True, 'VISION_STREAM_DRIVER', 32, 20, False), + (True, 'VISION_STREAM_ROAD', 31, 20, False), + (True, 'VISION_STREAM_ROAD', 32, 21, False), +]) +def test_only_opted_in_even_road_frames_use_region_upload(camera, monkeypatch, enabled, stream, stride, height, region_expected): + from msgq.visionipc import VisionStreamType + + view, _ = camera + view._use_roi_upload = enabled + view._use_egl = False + view._stream_type = getattr(VisionStreamType, stream) + view._switching = False + view._onroad_reentry_pending = False + view.frame.stride = stride + view.frame.height = height + view.client = SimpleNamespace(recv=lambda **_: None, is_connected=lambda: True) + monkeypatch.setattr(view, '_calc_frame_matrix', lambda _: np.eye(3)) + calls = [] + monkeypatch.setattr(view, '_render_textures', lambda *args: calls.append(args)) + rect = rl.Rectangle(0, 0, 24, 20) + + view._render(rect) + + assert len(calls) == 1 + assert len(calls[0]) == (3 if region_expected else 2)