def test_get_mesh_grid_as_point_cloud_3x3square(): """ Sample a regular grid and return the (x,y) coordinates of the sampled points. """ min_x = -3 # integer, minimum x-coordinate of 2D grid max_x = -1 # integer, maximum x-coordinate of 2D grid min_y = 2 # integer, minimum y-coordinate of 2D grid max_y = 4 # integer, maximum y-coordinate of 2D grid # return pts, a Numpy array of shape (N,2) pts = get_mesh_grid_as_point_cloud(min_x, max_x, min_y, max_y, downsample_factor=1.0) assert pts.shape == (9, 2) gt_pts = np.array([ [-3.0, 2.0], [-2.0, 2.0], [-1.0, 2.0], [-3.0, 3.0], [-2.0, 3.0], [-1.0, 3.0], [-3.0, 4.0], [-2.0, 4.0], [-1.0, 4.0], ]) assert np.allclose(gt_pts, pts)
def populate_frustum_voxels(planes: Sequence[np.ndarray], fig: Figure, axis_pair: str) -> Figure: """ Generate grid in xy plane, and then treat it as grid in xz (ground) plane in camera coordinate system. Args: planes: list of length 5. Each list element is a Numpy array of shape (4,) representing the equation of a plane, e.g. (a,b,c,d) in ax+by+cz=d fig: Mayavi figure to draw on axis_pair: Either "xz" or "yz" Returns: Mayavi figure """ sparse_xz_voxel_grid = get_mesh_grid_as_point_cloud(-20, 20, 0, 40, downsample_factor=0.1) sparse_voxel_grid = np.zeros((sparse_xz_voxel_grid.shape[0], 3)) if axis_pair == "xz": sparse_voxel_grid[:, 0] = sparse_xz_voxel_grid[:, 0] sparse_voxel_grid[:, 2] = sparse_xz_voxel_grid[:, 1] elif axis_pair == "yz": sparse_voxel_grid[:, 1] = sparse_xz_voxel_grid[:, 0] sparse_voxel_grid[:, 2] = sparse_xz_voxel_grid[:, 1] # keep only the points that have signed distance > 0 (inside the frustum, plane # normals also point into the frustum) for plane in planes: signed_d = np.matmul(sparse_voxel_grid, plane[:3]) + plane[3] sparse_voxel_grid = sparse_voxel_grid[np.where(signed_d > 0)] plot_points_3D_mayavi(sparse_voxel_grid, fig, fixed_color=(1, 0, 0)) return fig
def test_get_mesh_grid_as_point_cloud_downsample() -> None: """ Sample a regular grid and return the (x,y) coordinates of the sampled points. """ min_x = -3 # integer, minimum x-coordinate of 2D grid max_x = 0 # integer, maximum x-coordinate of 2D grid min_y = 2 # integer, minimum y-coordinate of 2D grid max_y = 5 # integer, maximum y-coordinate of 2D grid # return pts, a Numpy array of shape (N,2) pts = get_mesh_grid_as_point_cloud(min_x, max_x, min_y, max_y, downsample_factor=2.0) assert pts.shape == (4, 2) gt_pts = [[-3.0, 2.0], [0.0, 2.0], [-3.0, 5.0], [0.0, 5.0]] assert np.allclose(gt_pts, pts)
def render_global_city_map_bev( lane_centerlines: LaneCenterline, driveable_areas: np.ndarray, city_name: str, avm: MapProtocol, plot_rasterized_roi: bool = True, plot_vectormap_roi: bool = False, centerline_color_scheme: str = "constant", ) -> None: """ Assume indegree and outdegree are always less than 7. Since 1 pixel-per meter resolution is very low, we upsample the resolution by some constant factor. Args: lane_centerlines: Line centerline data driveable_areas: Driveable area data city_name: The city name (eg. "PIT") avm: The map plot_rasterized_roi: Whether to show the rasterized ROI plot_vectormap_roi: Whether to show the vector map ROI centerline_color_scheme: `"indegree"`, `"outdegree"`, or `"constant"` """ UPSAMPLE_FACTOR = 2 GRID_NUMBER_INTERVAL = 500 NUM_COLOR_BINS = 7 warnings.filterwarnings("error") im_h, im_w, xmin, xmax, ymin, ymax = _get_int_image_bounds_from_city_coords( driveable_areas, lane_centerlines) rendered_image = np.ones( (im_h * UPSAMPLE_FACTOR, im_w * UPSAMPLE_FACTOR, 3)) image_to_city_se2 = SE2(rotation=np.eye(2), translation=np.array([-xmin, -ymin])) if plot_rasterized_roi: grid_2d_pts = get_mesh_grid_as_point_cloud(xmin + 1, xmax - 1, ymin + 1, ymax - 1) pink = np.array([180.0, 105.0, 255.0]) / 255 roi_2d_pts = avm.remove_non_driveable_area_points( grid_2d_pts, city_name) roi_2d_pts = image_to_city_se2.transform_point_cloud(roi_2d_pts) roi_2d_pts *= UPSAMPLE_FACTOR roi_2d_pts = np.round(roi_2d_pts).astype(np.int32) for pt in roi_2d_pts: row = pt[1] col = pt[0] rendered_image[row, col, :] = pink if plot_vectormap_roi: driveable_areas = copy.deepcopy(driveable_areas) for da_idx, da in enumerate(driveable_areas): da = image_to_city_se2.transform_point_cloud(da[:, :2]) rendered_image = draw_polygon_cv2(da * UPSAMPLE_FACTOR, rendered_image, pink) for x in range(0, im_w * UPSAMPLE_FACTOR, GRID_NUMBER_INTERVAL): for y in range(0, im_h * UPSAMPLE_FACTOR, GRID_NUMBER_INTERVAL): coords_str = f"{x}_{y}" cv2.putText( rendered_image, coords_str, (x, y), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA, bottomLeftOrigin=True, ) # Color the graph max_outdegree = 0 max_indegree = 0 colors_arr = _get_opencv_green_to_red_colormap(NUM_COLOR_BINS) blue = [0, 0, 1][::-1] for lane_id, lane_obj in lane_centerlines.items(): centerline_2d = lane_obj.centerline centerline_2d = image_to_city_se2.transform_point_cloud(centerline_2d) centerline_2d = np.round(centerline_2d).astype(np.int32) preds = lane_obj.predecessors succs = lane_obj.successors indegree = 0 if preds is None else len(preds) outdegree = 0 if succs is None else len(succs) if indegree > max_indegree: max_indegree = indegree print("max indegree", indegree) if outdegree > max_outdegree: max_outdegree = outdegree print("max outdegree", outdegree) if centerline_color_scheme == "indegree": color = colors_arr[indegree] elif centerline_color_scheme == "outdegree": color = colors_arr[outdegree] elif centerline_color_scheme == "constant": color = blue draw_polyline_cv2( centerline_2d * UPSAMPLE_FACTOR, rendered_image, color, im_h * UPSAMPLE_FACTOR, im_w * UPSAMPLE_FACTOR, ) # provide colormap in corner for i in range(NUM_COLOR_BINS): rendered_image[0, i, :] = colors_arr[i] rendered_image = np.flipud(rendered_image) rendered_image *= 255 rendered_image = rendered_image.astype(np.uint8) cur_datetime = generate_datetime_string() img_save_fpath = f"{city_name}_{centerline_color_scheme}_{cur_datetime}.png" cv2.imwrite(filename=img_save_fpath, img=rendered_image) warnings.resetwarnings()