def viz_trajectory(xy_pred: np.ndarray, centerline: np.ndarray): plt.figure(figsize=(8, 7)) visualize_centerline(centerline) plt.plot( xy[:, 0], xy[:, 1], "-", color="#d33e4c", alpha=1, linewidth=3, zorder=15, ) final_x = xy[-1, 0] final_y = xy[-1, 1] plt.plot( final_x, final_y, "o", color="#d33e4c", alpha=1, markersize=10, zorder=15, ) plt.xlabel("Map X") plt.ylabel("Map Y") plt.axis("off") plt.show()
def plot_scene(args, seq, seq_agents_df, map_feature_row): """Function that plots the centerlines and all agent trajectories.""" candidate_centerlines = map_feature_row["CANDIDATE_CENTERLINES"] # Visualize the map centerlines for centerline_coords in candidate_centerlines: visualize_centerline(centerline_coords) # Plot the all the other agents for other_id in seq_agents_df["TRACK_ID"].unique(): plot_agent_track(track_id=other_id, seq_agents_df=seq_agents_df, colour="#289BD4", line_width=3, alpha=0.5) # Plot the trajectory of the real "AGENT" agent_track_id = seq_agents_df[seq_agents_df["OBJECT_TYPE"] == "AGENT"]["TRACK_ID"].unique()[0] agent_xy = plot_agent_track(track_id=agent_track_id, seq_agents_df=seq_agents_df, colour="#D4CF53", line_width=3, alpha=1) # Plot the trajectory of this agent track_id = map_feature_row["TRACK_ID"] agent_track = seq_agents_df[seq_agents_df["TRACK_ID"] == track_id].values plot_agent_track(track_id=track_id, seq_agents_df=seq_agents_df, colour="#d33e4c", line_width=3, alpha=1) return agent_xy
def get_candidate_centerlines_for_traj( self, xy: np.ndarray, city_name: str, viz: bool = False, max_search_radius: float = 50.0) -> List[np.ndarray]: """ Get centerline candidates upto a threshold. . Algorithm: 1. Take the lanes in the bubble of last obs coordinate 2. Extend before and after considering all possible candidates 3. Get centerlines with max distance along centerline Args: xy: trajectory of shape (N, 2). city_name viz: Visualize Returns: candidate_centerlines: List of candidate centerlines """ # Get all lane candidates within a bubble manhattan_threshold = 2.5 curr_lane_candidates = self.get_lane_ids_in_xy_bbox( xy[-1, 0], xy[-1, 1], city_name, manhattan_threshold) # Keep expanding the bubble until at least 1 lane is found while len(curr_lane_candidates ) < 1 and manhattan_threshold < max_search_radius: manhattan_threshold *= 2 curr_lane_candidates = self.get_lane_ids_in_xy_bbox( xy[-1, 0], xy[-1, 1], city_name, manhattan_threshold) assert len(curr_lane_candidates) > 0, "No nearby lanes found!!" # Set dfs threshold displacement = np.sqrt((xy[0, 0] - xy[-1, 0])**2 + (xy[0, 1] - xy[-1, 1])**2) dfs_threshold = displacement * 2.0 # DFS to get all successor and predecessor candidates obs_pred_lanes: List[Sequence[int]] = [] for lane in curr_lane_candidates: candidates_future = self.dfs(lane, city_name, 0, dfs_threshold) candidates_past = self.dfs(lane, city_name, 0, dfs_threshold, True) # Merge past and future for past_lane_seq in candidates_past: for future_lane_seq in candidates_future: assert past_lane_seq[-1] == future_lane_seq[ 0], "Incorrect DFS for candidate lanes past and future" obs_pred_lanes.append(past_lane_seq + future_lane_seq[1:]) # Removing overlapping lanes obs_pred_lanes = remove_overlapping_lane_seq(obs_pred_lanes) # Remove unnecessary extended predecessors obs_pred_lanes = self.remove_extended_predecessors( obs_pred_lanes, xy, city_name) # Getting candidate centerlines candidate_cl = self.get_cl_from_lane_seq(obs_pred_lanes, city_name) # Reduce the number of candidates based on distance travelled along the centerline candidate_centerlines = filter_candidate_centerlines(xy, candidate_cl) # If no candidate found using above criteria, take the onces along with travel is the maximum if len(candidate_centerlines) < 1: candidate_centerlines = get_centerlines_most_aligned_with_trajectory( xy, candidate_cl) if viz: plt.figure(0, figsize=(8, 7)) for centerline_coords in candidate_centerlines: visualize_centerline(centerline_coords) plt.plot(xy[:, 0], xy[:, 1], "-", color="#d33e4c", alpha=1, linewidth=1, zorder=15) final_x = xy[-1, 0] final_y = xy[-1, 1] plt.plot(final_x, final_y, "o", color="#d33e4c", alpha=1, markersize=7, zorder=15) plt.xlabel("Map X") plt.ylabel("Map Y") plt.axis("off") plt.title("Number of candidates = {}".format( len(candidate_centerlines))) plt.show() return candidate_centerlines
def get_candidate_centerlines_for_trajectory( self, xy: np.ndarray, city_name: str, avm: ArgoverseMap, viz: bool = False, max_search_radius: float = 50.0, seq_len: int = 50, max_candidates: int = 10, mode: str = "test", ) -> List[np.ndarray]: """Get centerline candidates upto a threshold. Algorithm: 1. Take the lanes in the bubble of last observed coordinate 2. Extend before and after considering all possible candidates 3. Get centerlines based on point in polygon score. Args: xy: Trajectory coordinates, city_name: City name, avm: Argoverse map_api instance, viz: Visualize candidate centerlines, max_search_radius: Max search radius for finding nearby lanes in meters, seq_len: Sequence length, max_candidates: Maximum number of centerlines to return, mode: train/val/test mode Returns: candidate_centerlines: List of candidate centerlines """ # Get all lane candidates within a bubble curr_lane_candidates = avm.get_lane_ids_in_xy_bbox( xy[-1, 0], xy[-1, 1], city_name, self._MANHATTAN_THRESHOLD) # Keep expanding the bubble until at least 1 lane is found while (len(curr_lane_candidates) < 1 and self._MANHATTAN_THRESHOLD < max_search_radius): self._MANHATTAN_THRESHOLD *= 2 curr_lane_candidates = avm.get_lane_ids_in_xy_bbox( xy[-1, 0], xy[-1, 1], city_name, self._MANHATTAN_THRESHOLD) assert len(curr_lane_candidates) > 0, "No nearby lanes found!!" # Set dfs threshold traj_len = xy.shape[0] # Assuming a speed of 50 mps, set threshold for traversing in the front and back dfs_threshold_front = (self._DFS_THRESHOLD_FRONT_SCALE * (seq_len + 1 - traj_len) / 10) dfs_threshold_back = self._DFS_THRESHOLD_BACK_SCALE * (traj_len + 1) / 10 # DFS to get all successor and predecessor candidates obs_pred_lanes: List[Sequence[int]] = [] for lane in curr_lane_candidates: candidates_future = avm.dfs(lane, city_name, 0, dfs_threshold_front) candidates_past = avm.dfs(lane, city_name, 0, dfs_threshold_back, True) # Merge past and future for past_lane_seq in candidates_past: for future_lane_seq in candidates_future: assert ( past_lane_seq[-1] == future_lane_seq[0] ), "Incorrect DFS for candidate lanes past and future" obs_pred_lanes.append(past_lane_seq + future_lane_seq[1:]) # Removing overlapping lanes obs_pred_lanes = remove_overlapping_lane_seq(obs_pred_lanes) # Sort lanes based on point in polygon score obs_pred_lanes, scores = self.sort_lanes_based_on_point_in_polygon_score( obs_pred_lanes, xy, city_name, avm) # If the best centerline is not along the direction of travel, re-sort if mode == "test": candidate_centerlines = self.get_heuristic_centerlines_for_test_set( obs_pred_lanes, xy, city_name, avm, max_candidates, scores) else: candidate_centerlines = avm.get_cl_from_lane_seq( [obs_pred_lanes[0]], city_name) if viz: plt.figure(0, figsize=(8, 7)) for centerline_coords in candidate_centerlines: visualize_centerline(centerline_coords) plt.plot( xy[:, 0], xy[:, 1], "-", color="#d33e4c", alpha=1, linewidth=3, zorder=15, ) final_x = xy[-1, 0] final_y = xy[-1, 1] plt.plot( final_x, final_y, "o", color="#d33e4c", alpha=1, markersize=10, zorder=15, ) plt.xlabel("Map X") plt.ylabel("Map Y") plt.axis("off") plt.title(f"Number of candidates = {len(candidate_centerlines)}") plt.show() return candidate_centerlines
def viz_trajectory(data): xy_pred = data[5] features_data = data[1] ground_truth_list = ["X", "Y"] ground_truth_idx = [ TEST_FEATURE_FORMAT[feature] for feature in ground_truth_list ] ground_truth = features_data[:, ground_truth_idx].astype('float64') oracle_centerline = data[2] plt.figure(figsize=(8, 7)) visualize_centerline(oracle_centerline) plt.plot( ground_truth[:21, 0], ground_truth[:21, 1], "-", color="b", alpha=1, linewidth=3, zorder=15, ) plt.plot( ground_truth[20:, 0], ground_truth[20:, 1], "-", color="k", alpha=1, linewidth=3, zorder=15, ) plt.plot( xy_pred[:, 0], xy_pred[:, 1], "-", color="#d33e4c", alpha=1, linewidth=3, zorder=15, ) final_x = xy_pred[-1, 0] final_y = xy_pred[-1, 1] plt.plot( final_x, final_y, "o", color="#d33e4c", alpha=1, markersize=10, zorder=15, ) plt.xlabel("Map X") plt.ylabel("Map Y") plt.axis("off") plt.axis("equal") plt.legend([ "Oracle Centerline", "Observed trajectory", "Ground Truth", "Predicted Trajectory", "End Point" ]) plt.title( "Trajectory forcasting with ADE = {:.2f} and FDE = {:.2f}".format( data[7][0], data[6][0])) plt.show()