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