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