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