def plot_nearby_halluc_lanes(
self,
ax: plt.Axes,
city_name: str,
query_x: float,
query_y: float,
patch_color: str = "r",
radius: float = 20,
) -> None:
"""
Plot lane segment for nearby lanes of the specified x, y location
Args:
query_bbox: An array of shape (4,) representing a 2d axis-aligned bounding box, with order
[xmin,xmax,ymin,ymax]
city_name: either 'MIA' for Miami or 'PIT' for Pittsburgh
"""
nearby_lane_ids = self.get_lane_ids_in_xy_bbox(query_x, query_y,
city_name, radius)
for nearby_lane_id in nearby_lane_ids:
halluc_lane_polygon = self.get_lane_segment_polygon(
nearby_lane_id, city_name)
plot_lane_segment_patch(halluc_lane_polygon,
ax,
color=patch_color,
alpha=0.3)
def test_plot_lane_segment_patch_smokescreen() -> None:
"""Test drawing lane segment with `matplotlib`."""
ax = plt.axes([1, 1, 1, 1])
polygon_pts = np.array([[-1, 0], [1, 0], [0, 1]])
color = "r"
alpha = 0.9
plot_lane_segment_patch(polygon_pts, ax, color, alpha)
plt.close("all")
def test_plot_lane_segment_patch_smokescreen():
"""
"""
ax = plt.axes([1, 1, 1, 1])
polygon_pts = np.array([[-1, 0], [1, 0], [0, 1]])
color = "r"
alpha = 0.9
plot_lane_segment_patch(polygon_pts, ax, color, alpha)
plt.close("all")
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 add_lane_segment_to_ax(
ax: plt.Axes,
lane_centerline: np.ndarray,
lane_polygon: np.ndarray,
patch_color: str,
xmin: float,
xmax: float,
ymin: float,
ymax: float,
) -> None:
"""Utility function to add a lane segment to an Matplotlib axis."""
plot_lane_segment_patch(lane_polygon, ax, color=patch_color, alpha=0.3)
def add_lane_segment_to_ax(
ax: plt.axes.Axis,
lane_centerline: np.ndarray,
lane_polygon: np.ndarray,
patch_color: str,
xmin: float,
xmax: float,
ymin: float,
ymax: float,
) -> None:
""""""
plot_lane_segment_patch(lane_polygon, ax, color=patch_color, alpha=0.3)
def verify_point_in_polygon_for_lanes():
"""
"""
avm = ArgoverseMap()
# ref_query_x = 422.
# ref_query_y = 1005.
ref_query_x = -662
ref_query_y = 2817
city_name = "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
fig = plt.figure(figsize=(22.5, 8))
ax = fig.add_subplot(111)
ax.scatter([query_x], [query_y], 100, color="k", marker=".")
occupied_lane_ids = avm.get_lane_segments_containing_xy(
query_x, query_y, city_name)
for occupied_lane_id in occupied_lane_ids:
halluc_lane_polygon = avm.get_lane_segment_polygon(
occupied_lane_id, city_name)
plot_lane_segment_patch(halluc_lane_polygon,
ax,
color="y",
alpha=0.3)
nearby_lane_ids = avm.get_lane_ids_in_xy_bbox(query_x, query_y,
city_name)
nearby_lane_ids = set(nearby_lane_ids) - set(occupied_lane_ids)
for nearby_lane_id in nearby_lane_ids:
halluc_lane_polygon = avm.get_lane_segment_polygon(
nearby_lane_id, city_name)
plot_lane_segment_patch(halluc_lane_polygon,
ax,
color="r",
alpha=0.3)
ax.axis("equal")
plt.show()
plt.close("all")
def plot_nearby_halluc_lanes(ax,
city_name,
adm,
query_x,
query_y,
patch_color="r",
radius=20):
"""
"""
nearby_lane_ids = avm.get_lane_ids_in_xy_bbox(query_x, query_y, city_name,
radius)
for nearby_lane_id in nearby_lane_ids:
halluc_lane_polygon = avm.get_lane_segment_polygon(
nearby_lane_id, city_name)
plot_lane_segment_patch(halluc_lane_polygon,
ax,
color=patch_color,
alpha=0.3)
plt.text(halluc_lane_polygon[:, 0].mean(),
halluc_lane_polygon[:, 1].mean(), str(nearby_lane_id))
def plot_nearby_halluc_lanes(
ax: plt.Axes,
city_name: str,
avm: ArgoverseMap,
query_x: float,
query_y: float,
patch_color: str = "r",
radius: float = 20.0,
) -> None:
"""Produce lane segment graphs for visual verification."""
nearby_lane_ids = avm.get_lane_ids_in_xy_bbox(query_x, query_y, city_name,
radius)
for nearby_lane_id in nearby_lane_ids:
halluc_lane_polygon = avm.get_lane_segment_polygon(
nearby_lane_id, city_name)
plot_lane_segment_patch(halluc_lane_polygon,
ax,
color=patch_color,
alpha=0.3)
plt.text(
halluc_lane_polygon[:, 0].mean(),
halluc_lane_polygon[:, 1].mean(),
str(nearby_lane_id),
)
def add_lane_segment_to_ax(ax, lane_centerline, lane_polygon, patch_color,
xmin, xmax, ymin, ymax):
"""
"""
plot_lane_segment_patch(lane_polygon, ax, color=patch_color, alpha=0.3)
def main(data_dir):
"""
"""
fnames = glob.glob(f"{data_dir}/*.csv")
fnames = [Path(fname).name for fname in fnames]
am = ArgoverseMap()
city_graph_dict = build_city_lane_graphs(am)
for fname in fnames:
# # very hard cases
# if int(Path(fname).stem) not in [
# 166633, 150381,11905, 136010, 49854, 27155]:
# continue
# # hard cases -- ,
# [174545,119781, 210709, 139445, 11381, 175883, 122703, 166633]: #23333,,124414]:
#
csv_fpath = f"{data_dir}/{fname}"
traj, city_name = get_traj_and_city_name_from_csv(csv_fpath)
plausible_start_ids = set()
lane_vote_dict = defaultdict(int)
for j, pt in enumerate(traj):
contained_ids = am.get_lane_segments_containing_xy(
pt[0], pt[1], city_name)
for id in contained_ids:
lane_vote_dict[id] += 1
plausible_start_ids.add(id)
plausible_start_ids = list(plausible_start_ids)
plausible_start_ids.sort()
paths = []
# START BFS IN ANY PLAUSIBLE LANE ID!
for start_id in plausible_start_ids:
paths.extend(
find_all_paths_from_src(city_graph_dict[city_name],
str(start_id),
max_depth=DFS_MAX_DEPTH))
paths = convert_str_lists_to_int_lists(paths)
paths = trim_paths_with_no_inliers(paths, lane_vote_dict)
paths = remove_repeated_paths(paths)
path_votes_dict = defaultdict(int)
for path_id, path in enumerate(paths):
for id in path:
path_votes_dict[path_id] += lane_vote_dict[id]
# find which path has most inliers
best_path_ids = get_dict_key_with_max_value(path_votes_dict)
# if they are all tied, take the shortest
best_path_lengths = [len(paths[id]) for id in best_path_ids]
min_best_path_length = min(best_path_lengths)
best_path_ids = [
id for id in best_path_ids
if len(paths[id]) == min_best_path_length
]
fig = plt.figure(figsize=(15, 15))
plt.axis("off")
ax = fig.add_subplot(111)
plot_all_nearby_lanes(am, ax, city_name, np.mean(traj[:, 0]),
np.mean(traj[:, 1]))
colors = ["g", "b", "r", "m"]
# then plot this path
for best_path_id in best_path_ids:
color = colors[best_path_id % len(colors)]
print(
"Candidate: ",
paths[best_path_id],
" with ",
path_votes_dict[best_path_id],
)
for lane_id in paths[best_path_id]:
polygon_3d = am.get_lane_segment_polygon(lane_id, city_name)
plot_lane_segment_patch(polygon_3d[:, :2], ax, color=color)
ax.text(np.mean(polygon_3d[:, 0]), np.mean(polygon_3d[:, 1]),
f"{lane_id}")
# just use one for now
break
# draw_lane_ids(plausible_start_ids, am, ax, city_name)
all_nearby_lane_ids = []
for path in paths:
all_nearby_lane_ids.extend(path)
draw_lane_ids(set(all_nearby_lane_ids), am, ax, city_name)
draw_traj(traj, ax)
xmin, ymin, xmax, ymax = compute_point_cloud_bbox(traj)
WINDOW_RADIUS_MARGIN = 10
xmin -= WINDOW_RADIUS_MARGIN
xmax += WINDOW_RADIUS_MARGIN
ymin -= WINDOW_RADIUS_MARGIN
ymax += WINDOW_RADIUS_MARGIN
ax.set_xlim([xmin, xmax])
ax.set_ylim([ymin, ymax])
plt.savefig(
f"/Users/johnlamb/Documents/argoverse-api/temp_files_oracle/{Path(fname).stem}.png"
)
plt.close("all")
