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 test_filter_candidate_centerlines(): """Test filter candidate centerlines""" # Test Case # 0 20 24 30 40 60 # * * 50 # | | # | | # | | # | | # | | 40 # (3) | | (2) # ^ ^ # ^ ^ # | | # | | # | | (1) # *--------<<<-----------|---------|--------<<<--------* 30 # | \ | # xxxxxxxx^x \ ^ # ^ x \(5) ^ # (4) | x \ | # *-------->>>-----------|---x-\---|-------->>>--------* 20 # | x \ | # | x \ | # | x \| # | x | 10 # | x | # ^ x ^ # ^ x ^ # | | # * * 0 # xy = np.array([ [35.0, 0.0], [35.0, 4.0], [35.0, 8.0], [35.0, 12.0], [35.0, 16.0], [33.0, 20.0], [31.0, 24.0], [29.0, 28.0], [25.0, 28.0], [21.0, 28.0], ]) cl1 = np.array([(60.0, 30.0), (0.0, 30.0)]) cl2 = np.array([(40.0, 0.0), (40.0, 50.0)]) cl3 = np.array([(30.0, 0.0), (30.0, 50.0)]) cl4 = np.array([(0.0, 20.0), (60.0, 20.0)]) cl5 = np.array([(40.0, 0.0), (40.0, 10.0), (30.0, 30.0), (0.0, 30.0)]) candidate_cl = [cl1, cl2, cl3, cl4, cl5] filtered_cl = sorted(filter_candidate_centerlines(xy, candidate_cl)) expected_cl = [cl5] for i in range(len(filtered_cl)): assert np.allclose(expected_cl[i], filtered_cl[i]), "Filtered centerlines wrong!"