From 1c11e2844860ec25670fd4fb91b5115dac2e884a Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Tue, 10 Jun 2025 12:29:25 +0800 Subject: [PATCH] ui: fix path self-intersections on hills (#35514) fix path self-intersections on hills --- selfdrive/ui/onroad/model_renderer.py | 96 ++++++++++++++------------- 1 file changed, 49 insertions(+), 47 deletions(-) diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 027b8342f3..1fb10e01cd 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -67,10 +67,6 @@ class ModelRenderer(Widget): self._clip_region = None self._rect = None - # Pre-allocated arrays for polygon conversion - self._temp_points_3d = np.empty((MAX_POINTS * 2, 3), dtype=np.float32) - self._temp_proj = np.empty((3, MAX_POINTS * 2), dtype=np.float32) - self._exp_gradient = { 'start': (0.0, 1.0), # Bottom of path 'end': (0.0, 0.0), # Top of path @@ -359,54 +355,60 @@ class ModelRenderer(Widget): if points.shape[0] == 0: return np.empty((0, 2), dtype=np.float32) - # Create left and right 3D points in one array - n_points = points.shape[0] - points_3d = self._temp_points_3d[:n_points * 2] - points_3d[:n_points, 0] = points_3d[n_points:, 0] = points[:, 0] - points_3d[:n_points, 1] = points[:, 1] - y_off - points_3d[n_points:, 1] = points[:, 1] + y_off - points_3d[:n_points, 2] = points_3d[n_points:, 2] = points[:, 2] + z_off - - # Single matrix multiplication for projections - proj = np.ascontiguousarray(self._temp_proj[:, :n_points * 2]) # Slice the pre-allocated array - np.dot(self._car_space_transform, points_3d.T, out=proj) - valid_z = np.abs(proj[2]) > 1e-6 - if not np.any(valid_z): + N = points.shape[0] + # Generate left and right 3D points in one array using broadcasting + offsets = np.array([[0, -y_off, z_off], [0, y_off, z_off]], dtype=np.float32) + points_3d = points[None, :, :] + offsets[:, None, :] # Shape: 2xNx3 + points_3d = points_3d.reshape(2 * N, 3) # Shape: (2*N)x3 + + # Transform all points to projected space in one operation + proj = self._car_space_transform @ points_3d.T # Shape: 3x(2*N) + proj = proj.reshape(3, 2, N) + left_proj = proj[:, 0, :] + right_proj = proj[:, 1, :] + + # Filter points where z is sufficiently large + valid_proj = (np.abs(left_proj[2]) >= 1e-6) & (np.abs(right_proj[2]) >= 1e-6) + if not np.any(valid_proj): return np.empty((0, 2), dtype=np.float32) # Compute screen coordinates - screen = proj[:2, valid_z] / proj[2, valid_z][None, :] - left_screen = screen[:, :n_points].T - right_screen = screen[:, n_points:].T + left_screen = left_proj[:2, valid_proj] / left_proj[2, valid_proj][None, :] + right_screen = right_proj[:2, valid_proj] / right_proj[2, valid_proj][None, :] + + # Define clip region bounds + clip = self._clip_region + x_min, x_max = clip.x, clip.x + clip.width + y_min, y_max = clip.y, clip.y + clip.height + + # Filter points within clip region + left_in_clip = ( + (left_screen[0] >= x_min) & (left_screen[0] <= x_max) & + (left_screen[1] >= y_min) & (left_screen[1] <= y_max) + ) + right_in_clip = ( + (right_screen[0] >= x_min) & (right_screen[0] <= x_max) & + (right_screen[1] >= y_min) & (right_screen[1] <= y_max) + ) + both_in_clip = left_in_clip & right_in_clip - # Ensure consistent shapes by re-aligning valid points - valid_points = np.minimum(left_screen.shape[0], right_screen.shape[0]) - if valid_points == 0: + if not np.any(both_in_clip): return np.empty((0, 2), dtype=np.float32) - left_screen = left_screen[:valid_points] - right_screen = right_screen[:valid_points] - - if self._clip_region: - clip = self._clip_region - bounds_mask = ( - (left_screen[:, 0] >= clip.x) & (left_screen[:, 0] <= clip.x + clip.width) & - (left_screen[:, 1] >= clip.y) & (left_screen[:, 1] <= clip.y + clip.height) & - (right_screen[:, 0] >= clip.x) & (right_screen[:, 0] <= clip.x + clip.width) & - (right_screen[:, 1] >= clip.y) & (right_screen[:, 1] <= clip.y + clip.height) - ) - if not np.any(bounds_mask): - return np.empty((0, 2), dtype=np.float32) - left_screen = left_screen[bounds_mask] - right_screen = right_screen[bounds_mask] - - if not allow_invert and left_screen.shape[0] > 1: - keep = np.concatenate(([True], np.diff(left_screen[:, 1]) < 0)) - left_screen = left_screen[keep] - right_screen = right_screen[keep] - if left_screen.shape[0] == 0: - return np.empty((0, 2), dtype=np.float32) - - return np.vstack((left_screen, right_screen[::-1])).astype(np.float32) + + # Select valid and clipped points + left_screen = left_screen[:, both_in_clip] + right_screen = right_screen[:, both_in_clip] + + # Handle Y-coordinate inversion on hills + if not allow_invert and left_screen.shape[1] > 1: + y = left_screen[1, :] # y-coordinates + keep = y == np.minimum.accumulate(y) + if not np.any(keep): + return np.empty((0, 2), dtype=np.float32) + left_screen = left_screen[:, keep] + right_screen = right_screen[:, keep] + + return np.vstack((left_screen.T, right_screen[:, ::-1].T)).astype(np.float32) @staticmethod def _map_val(x, x0, x1, y0, y1):