def verify_manhattan_search_functionality():
"""
Minimal example where we
"""
adm = ArgoverseMap()
# query_x = 254.
# query_y = 1778.
ref_query_x = 422.0
ref_query_y = 1005.0
city_name = "PIT" # 'MIA'
for trial_idx in range(10):
query_x = ref_query_x + (np.random.rand() - 0.5) * 10
query_y = ref_query_y + (np.random.rand() - 0.5) * 10
# query_x,query_y = (3092.49845414,1798.55426805)
query_x, query_y = (3112.80160113, 1817.07585338)
lane_segment_ids = avm.get_lane_ids_in_xy_bbox(query_x, query_y,
city_name, 5000)
fig = plt.figure(figsize=(22.5, 8))
ax = fig.add_subplot(111)
# ax.scatter([query_x], [query_y], 500, color='k', marker='.')
plot_lane_segment_patch(pittsburgh_bounds, ax, color="m", alpha=0.1)
if len(lane_segment_ids) > 0:
for i, lane_segment_id in enumerate(lane_segment_ids):
patch_color = "y" # patch_colors[i % 4]
lane_centerline = avm.get_lane_segment_centerline(
lane_segment_id, city_name)
test_x, test_y = lane_centerline.mean(axis=0)
inside = point_inside_polygon(n_poly_vertices,
pittsburgh_bounds[:, 0],
pittsburgh_bounds[:, 1], test_x,
test_y)
if inside:
halluc_lane_polygon = avm.get_lane_segment_polygon(
lane_segment_id, city_name)
xmin, ymin, xmax, ymax = find_lane_segment_bounds_in_table(
adm, city_name, lane_segment_id)
add_lane_segment_to_ax(ax, lane_centerline,
halluc_lane_polygon, patch_color,
xmin, xmax, ymin, ymax)
ax.axis("equal")
plt.show()
datetime_str = generate_datetime_string()
plt.savefig(f"{trial_idx}_{datetime_str}.jpg")
plt.close("all")
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()
def verify_halluc_lane_extent_index(enable_lane_boundaries=False):
"""
"""
avm = ArgoverseMap()
city_names = ["MIA", "PIT"]
for city_name in city_names:
# get all lane segment IDs inside of this city
lane_segment_ids = list(
avm.city_lane_centerlines_dict[city_name].keys())
for lane_segment_id in lane_segment_ids:
xmin, ymin, xmax, ymax = find_lane_segment_bounds_in_table(
adm, city_name, lane_segment_id)
predecessor_ids = avm.get_lane_segment_predecessor_ids(
lane_segment_id, city_name)
successor_ids = avm.get_lane_segment_successor_ids(
lane_segment_id, city_name)
(r_neighbor_id, l_neighbor_id) = avm.get_lane_segment_adjacent_ids(
lane_segment_id, city_name)
lane_centerline = avm.get_lane_segment_centerline(
lane_segment_id, city_name)
halluc_lane_polygon = avm.get_lane_segment_polygon(
lane_segment_id, city_name)
fig = plt.figure(figsize=(22.5, 8))
ax = fig.add_subplot(111)
# add the lane of interest
add_lane_segment_to_ax(ax, lane_centerline, halluc_lane_polygon,
"y", xmin, xmax, ymin, ymax)
if predecessor_ids is not None:
# add predecessors
for predecessor_id in predecessor_ids:
lane_centerline = avm.get_lane_segment_centerline(
predecessor_id, city_name)
halluc_lane_polygon = avm.get_lane_segment_polygon(
predecessor_id, city_name)
xmin, ymin, xmax, ymax = find_lane_segment_bounds_in_table(
adm, city_name, predecessor_id)
add_lane_segment_to_ax(ax, lane_centerline,
halluc_lane_polygon, "r", xmin,
xmax, ymin, ymax)
if successor_ids is not None:
# add successors
for successor_id in successor_ids:
lane_centerline = avm.get_lane_segment_centerline(
successor_id, city_name)
halluc_lane_polygon = avm.get_lane_segment_polygon(
successor_id, city_name)
xmin, ymin, xmax, ymax = find_lane_segment_bounds_in_table(
adm, city_name, successor_id)
add_lane_segment_to_ax(ax, lane_centerline,
halluc_lane_polygon, "b", xmin,
xmax, ymin, ymax)
# add left neighbor
if l_neighbor_id is not None:
lane_centerline = avm.get_lane_segment_centerline(
l_neighbor_id, city_name)
halluc_lane_polygon = avm.get_lane_segment_polygon(
l_neighbor_id, city_name)
xmin, ymin, xmax, ymax = find_lane_segment_bounds_in_table(
adm, city_name, l_neighbor_id)
add_lane_segment_to_ax(ax, lane_centerline,
halluc_lane_polygon, "g", xmin, xmax,
ymin, ymax)
# add right neighbor
if r_neighbor_id is not None:
lane_centerline = avm.get_lane_segment_centerline(
r_neighbor_id, city_name)
halluc_lane_polygon = avm.get_lane_segment_polygon(
r_neighbor_id, city_name)
xmin, ymin, xmax, ymax = find_lane_segment_bounds_in_table(
adm, city_name, r_neighbor_id)
add_lane_segment_to_ax(ax, lane_centerline,
halluc_lane_polygon, "m", xmin, xmax,
ymin, ymax)
if enable_lane_boundaries:
# Compare with Argo's proprietary, ground truth lane boundaries
gt_lane_polygons = avm.city_to_lane_polygons_dict[city_name]
for gt_lane_polygon in gt_lane_polygons:
dist = np.linalg.norm(
gt_lane_polygon.mean(axis=0)[:2] -
np.array([xmin, ymin]))
if dist < 30:
ax.plot(gt_lane_polygon[:, 0],
gt_lane_polygon[:, 1],
color="k",
alpha=0.3,
zorder=1)
ax.axis("equal")
plt.show()
datetime_str = generate_datetime_string()
plt.savefig(
f"lane_segment_id_{lane_segment_id}_@_{datetime_str}.jpg")
plt.close("all")
def test_datetime_smokescreen() -> None:
"""Basic test to ensure generate_datetime_string() returns a string."""
datetime_str = generate_datetime_string()
assert isinstance(datetime_str, str)