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