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()