def visualize_prediction(ax,
prediction_output_dict,
dt,
max_hl,
ph,
robot_node=None,
map=None,
**kwargs):
prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories(
prediction_output_dict, dt, max_hl, ph, map=map)
assert (len(prediction_dict.keys()) <= 1)
if len(prediction_dict.keys()) == 0:
return
ts_key = list(prediction_dict.keys())[0]
prediction_dict = prediction_dict[ts_key]
histories_dict = histories_dict[ts_key]
futures_dict = futures_dict[ts_key]
if map is not None:
ax.imshow(map.as_image(), origin='lower', alpha=0.5)
plot_trajectories(ax, prediction_dict, histories_dict, futures_dict,
*kwargs)
def compute_batch_statistics(prediction_output_dict, dt, max_hl, ph, map=None, best_of=False):
(prediction_dict,
_,
futures_dict) = prediction_output_to_trajectories(prediction_output_dict, dt, max_hl, ph, prune_ph_to_future=True)
batch_error_dict = {'mse': list(), 'fde': list(), 'kde': list(), 'obs_viols': list()}
for t in prediction_dict.keys():
for node in prediction_dict[t].keys():
mse_errors = compute_mse(prediction_dict[t][node], futures_dict[t][node])
fde_errors = compute_fde(prediction_dict[t][node], futures_dict[t][node])
kde_ll = compute_kde_nll(prediction_dict[t][node], futures_dict[t][node])
if map is not None:
obs_viols = compute_obs_violations(prediction_dict[t][node], map)
else:
obs_viols = 0
if best_of:
mse_errors = np.min(mse_errors)
fde_errors = np.min(fde_errors)
kde_ll = np.min(kde_ll)
batch_error_dict['mse'].append(mse_errors)
batch_error_dict['fde'].append(fde_errors)
batch_error_dict['kde'].append(kde_ll)
batch_error_dict['obs_viols'].append(obs_viols)
return (np.hstack(batch_error_dict['mse']),
np.hstack(batch_error_dict['fde']),
np.hstack(batch_error_dict['kde']),
np.hstack(batch_error_dict['obs_viols']))
def plot_vehicle_nice_mv_robot(ax,
predictions,
dt,
max_hl=10,
ph=6,
map=None,
x_min=0,
y_min=0):
prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories(
predictions, dt, max_hl, ph, map=map)
assert (len(prediction_dict.keys()) <= 1)
if len(prediction_dict.keys()) == 0:
return
ts_key = list(prediction_dict.keys())[0]
prediction_dict = prediction_dict[ts_key]
histories_dict = histories_dict[ts_key]
futures_dict = futures_dict[ts_key]
if map is not None:
ax.imshow(map.fdata, origin='lower', alpha=0.5)
cmap = ['k', 'b', 'y', 'g', 'r']
line_alpha = 0.7
line_width = 0.2
edge_width = 2
circle_edge_width = 0.5
node_circle_size = 0.3
node_list = sorted(histories_dict.keys(), key=lambda x: x.length)
for node in node_list:
h = node.get(ts_key, {'heading': ['value']})[0, 0]
history_org = histories_dict[node] + np.array([
x_min, y_min
]) + node.length / 2 * np.array([np.cos(h), np.sin(h)])
future = futures_dict[node] + np.array([
x_min, y_min
]) + node.length * np.array([np.cos(h), np.sin(h)])
ax.plot(
future[:, 0],
future[:, 1],
'--o',
c='#F05F78',
linewidth=4,
markersize=3,
zorder=650,
path_effects=[pe.Stroke(linewidth=5, foreground='k'),
pe.Normal()])
r_img = rotate(robot,
node.get(ts_key, {'heading': ['value']})[0, 0] * 180 /
np.pi,
reshape=True)
oi = OffsetImage(r_img, zoom=0.08, zorder=700)
veh_box = AnnotationBbox(oi, (history_org[-1, 0], history_org[-1, 1]),
frameon=False)
veh_box.zorder = 700
ax.add_artist(veh_box)
def plot_vehicle_mm(ax,
predictions,
dt,
max_hl=10,
ph=6,
map=None,
x_min=0,
y_min=0):
prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories(
predictions, dt, max_hl, ph, map=map)
assert (len(prediction_dict.keys()) <= 1)
if len(prediction_dict.keys()) == 0:
return
ts_key = list(prediction_dict.keys())[0]
prediction_dict = prediction_dict[ts_key]
histories_dict = histories_dict[ts_key]
futures_dict = futures_dict[ts_key]
if map is not None:
ax.imshow(map.fdata, origin='lower', alpha=0.5)
cmap = ['k', 'b', 'y', 'g', 'r']
line_alpha = 0.7
line_width = 0.2
edge_width = 2
circle_edge_width = 0.5
node_circle_size = 0.5
a = []
i = 0
node_list = sorted(histories_dict.keys(), key=lambda x: x.length)
for node in node_list:
history = histories_dict[node] + np.array([x_min, y_min])
future = futures_dict[node] + np.array([x_min, y_min])
predictions = prediction_dict[node] + np.array([x_min, y_min])
if node.type.name == 'VEHICLE':
for sample_num in range(prediction_dict[node].shape[0]):
ax.plot(predictions[sample_num, :, 0],
predictions[sample_num, :, 1],
'ko-',
zorder=620,
markersize=5,
linewidth=3,
alpha=0.7)
else:
for sample_num in range(prediction_dict[node].shape[0]):
ax.plot(predictions[sample_num, :, 0],
predictions[sample_num, :, 1],
'ko-',
zorder=620,
markersize=2,
linewidth=1,
alpha=0.7)
def compute_batch_statistics(prediction_output_dict,
dt,
max_hl,
ph,
node_type_enum,
kde=True,
obs=False,
map=None,
prune_ph_to_future=False,
best_of=False):
(prediction_dict, _, futures_dict) = prediction_output_to_trajectories(
prediction_output_dict,
dt,
max_hl,
ph,
prune_ph_to_future=prune_ph_to_future)
batch_error_dict = dict()
for node_type in node_type_enum:
batch_error_dict[node_type] = {
'ade': list(),
'fde': list(),
'kde': list(),
'obs_viols': list()
}
for t in prediction_dict.keys():
for node in prediction_dict[t].keys():
ade_errors = compute_ade(prediction_dict[t][node],
futures_dict[t][node])
fde_errors = compute_fde(prediction_dict[t][node],
futures_dict[t][node])
if kde:
kde_ll = compute_kde_nll(prediction_dict[t][node],
futures_dict[t][node])
else:
kde_ll = 0
if obs:
obs_viols = compute_obs_violations(prediction_dict[t][node],
map)
else:
obs_viols = 0
if best_of:
ade_errors = np.min(ade_errors, keepdims=True)
fde_errors = np.min(fde_errors, keepdims=True)
kde_ll = np.min(kde_ll)
batch_error_dict[node.type]['ade'].extend(list(ade_errors))
batch_error_dict[node.type]['fde'].extend(list(fde_errors))
batch_error_dict[node.type]['kde'].extend([kde_ll])
batch_error_dict[node.type]['obs_viols'].extend([obs_viols])
return batch_error_dict
def compute_batch_statistics(prediction_output_dict,
max_hl,
ph,
node_type_enum,
kde=False,
obs=False,
map=None,
prune_ph_to_future=False):
(prediction_dict, _, futures_dict) = prediction_output_to_trajectories(
prediction_output_dict,
max_hl,
ph,
prune_ph_to_future=prune_ph_to_future)
batch_error_dict = dict()
for node_type in node_type_enum:
batch_error_dict[node_type.name] = {'mm_ade': list(), 'mm_fde': list()}
if kde:
batch_error_dict[node_type.name]['nll'] = list()
if obs:
batch_error_dict[node_type.name]['obs_viols'] = list()
for t in prediction_dict.keys():
for node in prediction_dict[t].keys():
node_type_name = node.type.name
mm_prediction = prediction_dict[t][node].mode_mode().unsqueeze(0)
mm_ade_errors = compute_ade(mm_prediction, futures_dict[t][node])
mm_fde_errors = compute_fde(mm_prediction, futures_dict[t][node])
if kde:
nll = compute_nll(prediction_dict[t][node],
futures_dict[t][node])
if obs:
obs_viols = compute_obs_violations(prediction_dict[t][node],
map)
batch_error_dict[node_type_name]['mm_ade'].extend(
list(mm_ade_errors))
batch_error_dict[node_type_name]['mm_fde'].extend(
list(mm_fde_errors))
if kde:
batch_error_dict[node_type_name]['nll'].append(nll)
if obs:
batch_error_dict[node_type_name]['obs_viols'].append(obs_viols)
return batch_error_dict
def compute_mintopk_statistics(prediction_output_dict,
max_hl,
ph,
node_type_enum,
prune_ph_to_future=False):
(prediction_dict, _, futures_dict) = prediction_output_to_trajectories(
prediction_output_dict,
max_hl,
ph,
prune_ph_to_future=prune_ph_to_future)
batch_error_dict = dict()
for node_type in node_type_enum:
batch_error_dict[node_type.name] = {
'min_ade_k': list(),
'min_fde_k': list()
}
for t in prediction_dict.keys():
for node in prediction_dict[t].keys():
node_type_name = node.type.name
gaussian_means = prediction_dict[t][
node].component_distribution.mean[:, 0, :, :2]
component_pis = prediction_dict[t][node].pis
rank_order = torch.argsort(component_pis, descending=True)
ranked_predictions = torch.transpose(gaussian_means, 0,
1)[rank_order]
min_ade_errors = compute_mink_ade(ranked_predictions,
futures_dict[t][node])
min_fde_errors = compute_mink_fde(ranked_predictions,
futures_dict[t][node])
batch_error_dict[node_type_name]['min_ade_k'].append(
np.array(min_ade_errors))
batch_error_dict[node_type_name]['min_fde_k'].append(
np.array(min_fde_errors))
return batch_error_dict
def visualize_prediction(ax,
pred_dists,
dt,
max_hl,
ph,
robot_node=None,
map=None,
x_min=0,
y_min=0,
**kwargs):
prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories(
pred_dists, max_hl, ph, map=map)
assert (len(prediction_dict.keys()) <= 1)
if len(prediction_dict.keys()) == 0:
return
ts_key = list(prediction_dict.keys())[0]
prediction_dict = prediction_dict[ts_key]
histories_dict = histories_dict[ts_key]
futures_dict = futures_dict[ts_key]
if robot_node is not None:
histories_dict[robot_node] = robot_node.get(
np.array([ts_key - max_hl, ts_key]), {'position': ['x', 'y']})
futures_dict[robot_node] = robot_node.get(
np.array([ts_key + 1, ts_key + ph]), {'position': ['x', 'y']})
if map is not None:
ax.imshow(map.as_image(), origin='lower', alpha=0.5)
plot_trajectories(ax,
prediction_dict,
histories_dict,
futures_dict,
x_min=x_min,
y_min=y_min,
**kwargs)
timesteps = np.arange(scene.timesteps)
predictions = eval_stg.predict(scene,
timesteps,
ph,
num_samples=2000,
min_future_timesteps=8,
z_mode=False,
gmm_mode=False,
full_dist=False)
if not predictions:
continue
prediction_dict, _, _ = utils.prediction_output_to_trajectories(
predictions,
scene.dt,
max_hl,
ph,
prune_ph_to_future=False)
eval_road_viols_batch = []
for t in prediction_dict.keys():
for node in prediction_dict[t].keys():
if node.type == args.node_type:
viols = compute_road_violations(
prediction_dict[t][node],
scene.map[args.node_type],
channel=0)
if viols == 2000:
viols = 0
eval_road_viols_batch.append(viols)
def plot_vehicle_nice(ax,
predictions,
dt,
max_hl=10,
ph=6,
map=None,
x_min=0,
y_min=0):
prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories(
predictions, dt, max_hl, ph, map=map)
assert (len(prediction_dict.keys()) <= 1)
if len(prediction_dict.keys()) == 0:
return
ts_key = list(prediction_dict.keys())[0]
prediction_dict = prediction_dict[ts_key]
histories_dict = histories_dict[ts_key]
futures_dict = futures_dict[ts_key]
if map is not None:
ax.imshow(map.fdata, origin='lower', alpha=0.5)
cmap = ['k', 'b', 'y', 'g', 'r']
line_alpha = 0.7
line_width = 0.2
edge_width = 2
circle_edge_width = 0.5
node_circle_size = 0.3
a = []
i = 0
node_list = sorted(histories_dict.keys(), key=lambda x: x.length)
for node in node_list:
history = histories_dict[node] + np.array([x_min, y_min])
future = futures_dict[node] + np.array([x_min, y_min])
predictions = prediction_dict[node] + np.array([x_min, y_min])
if node.type.name == 'VEHICLE':
# ax.plot(history[:, 0], history[:, 1], 'ko-', linewidth=1)
ax.plot(future[:, 0],
future[:, 1],
'w--o',
linewidth=4,
markersize=3,
zorder=650,
path_effects=[
pe.Stroke(linewidth=5, foreground='k'),
pe.Normal()
])
for t in range(predictions.shape[1]):
sns.kdeplot(predictions[:, t, 0],
predictions[:, t, 1],
ax=ax,
shade=True,
shade_lowest=False,
color=line_colors[i % len(line_colors)],
zorder=600,
alpha=0.8)
vel = node.get(ts_key, {'velocity': ['x', 'y']})
h = np.arctan2(vel[0, 1], vel[0, 0])
r_img = rotate(cars[i % len(cars)],
node.get(ts_key, {'heading': ['value']})[0, 0] *
180 / np.pi,
reshape=True)
oi = OffsetImage(r_img, zoom=0.035, zorder=700)
veh_box = AnnotationBbox(oi, (history[-1, 0], history[-1, 1]),
frameon=False)
veh_box.zorder = 700
ax.add_artist(veh_box)
i += 1
else:
# ax.plot(history[:, 0], history[:, 1], 'k--')
for t in range(predictions.shape[1]):
sns.kdeplot(predictions[:, t, 0],
predictions[:, t, 1],
ax=ax,
shade=True,
shade_lowest=False,
color='b',
zorder=600,
alpha=0.8)
ax.plot(future[:, 0],
future[:, 1],
'w--',
zorder=650,
path_effects=[
pe.Stroke(linewidth=edge_width, foreground='k'),
pe.Normal()
])
# Current Node Position
circle = plt.Circle((history[-1, 0], history[-1, 1]),
node_circle_size,
facecolor='g',
edgecolor='k',
lw=circle_edge_width,
zorder=3)
ax.add_artist(circle)
def plot_vehicle_nice_mv(ax,
predictions,
dt,
max_hl=10,
ph=6,
map=None,
x_min=0,
y_min=0):
prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories(
predictions, dt, max_hl, ph, map=map)
assert (len(prediction_dict.keys()) <= 1)
if len(prediction_dict.keys()) == 0:
return
ts_key = list(prediction_dict.keys())[0]
prediction_dict = prediction_dict[ts_key]
histories_dict = histories_dict[ts_key]
futures_dict = futures_dict[ts_key]
if map is not None:
ax.imshow(map.fdata, origin='lower', alpha=0.5)
cmap = ['k', 'b', 'y', 'g', 'r']
line_alpha = 0.7
line_width = 0.2
edge_width = 2
circle_edge_width = 0.5
node_circle_size = 0.3
a = []
i = 0
node_list = sorted(histories_dict.keys(), key=lambda x: x.length)
for node in node_list:
h = node.get(ts_key, {'heading': ['value']})[0, 0]
history_org = histories_dict[node] + np.array([x_min, y_min])
history = histories_dict[node] + np.array([
x_min, y_min
]) + node.length * np.array([np.cos(h), np.sin(h)])
future = futures_dict[node] + np.array([
x_min, y_min
]) + node.length * np.array([np.cos(h), np.sin(h)])
predictions = prediction_dict[node] + np.array([
x_min, y_min
]) + node.length * np.array([np.cos(h), np.sin(h)])
if node.type.name == 'VEHICLE':
for t in range(predictions.shape[1]):
sns.kdeplot(predictions[:, t, 0],
predictions[:, t, 1],
ax=ax,
shade=True,
shade_lowest=False,
color=line_colors[i % len(line_colors)],
zorder=600,
alpha=1.0)
r_img = rotate(cars[i % len(cars)],
node.get(ts_key, {'heading': ['value']})[0, 0] *
180 / np.pi,
reshape=True)
oi = OffsetImage(r_img, zoom=0.08, zorder=700)
veh_box = AnnotationBbox(oi,
(history_org[-1, 0], history_org[-1, 1]),
frameon=False)
veh_box.zorder = 700
ax.add_artist(veh_box)
i += 1
else:
for t in range(predictions.shape[1]):
sns.kdeplot(predictions[:, t, 0],
predictions[:, t, 1],
ax=ax,
shade=True,
shade_lowest=False,
color='b',
zorder=600,
alpha=0.8)
# Current Node Position
circle = plt.Circle((history[-1, 0], history[-1, 1]),
node_circle_size,
facecolor='g',
edgecolor='k',
lw=circle_edge_width,
zorder=3)
ax.add_artist(circle)
def plot_vehicle_nice(ax,
predictions,
scene,
max_hl=10,
ph=6,
map=None,
x_min=0,
y_min=0,
pi_alpha=False,
line_alpha=0.8,
line_width=0.2,
edge_width=2,
circle_edge_width=0.5,
node_circle_size=0.3,
car_img_zoom=0.01,
plot_prediction=True,
plot_future=True,
at_timestep=0):
prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories(
predictions, max_hl, ph, map=map)
assert (len(prediction_dict.keys()) <= 1)
if len(prediction_dict.keys()) == 0:
return
ts_key = list(prediction_dict.keys())[0]
prediction_dict = prediction_dict[ts_key]
histories_dict = histories_dict[ts_key]
futures_dict = futures_dict[ts_key]
if map is not None:
ax.imshow(map.fdata, origin='lower', alpha=0.5)
cmap = ['k', 'b', 'y', 'g', 'r']
a = []
i = 0
node_list = sorted(histories_dict.keys(), key=lambda x: x.id)
for node in node_list:
history = histories_dict[node] + np.array([x_min, y_min])
future = futures_dict[node] + np.array([x_min, y_min])
predictions = prediction_dict[node]
if node.type.name == 'VEHICLE':
# ax.plot(history[:, 0], history[:, 1], 'ko-', linewidth=1)
if plot_future:
ax.plot(future[at_timestep:, 0],
future[at_timestep:, 1],
'w--o',
linewidth=4,
markersize=3,
zorder=650,
path_effects=[
pe.Stroke(linewidth=5, foreground='k'),
pe.Normal()
])
if plot_prediction:
pis = predictions.pis
for component in range(
predictions.mixture_distribution.param_shape[-1]):
if pi_alpha:
line_alpha = pis[component].item()
ax.plot(
predictions.component_distribution.mean[at_timestep:,
0, component,
0] + x_min,
predictions.component_distribution.mean[at_timestep:,
0, component,
1] + y_min,
color=cmap[1],
linewidth=line_width,
alpha=line_alpha,
zorder=600)
vel = node.get(np.array([ts_key + at_timestep]),
{'velocity': ['x', 'y']})
h = np.arctan2(vel[0, 1], vel[0, 0])
r_img = rotate(cars[i % len(cars)],
node.get(np.array([ts_key + at_timestep]),
{'heading': ['°']})[0, 0] * 180 / np.pi,
reshape=True)
oi = OffsetImage(r_img, zoom=car_img_zoom, zorder=700)
if at_timestep > 0:
veh_box = AnnotationBbox(
oi,
(future[at_timestep - 1, 0], future[at_timestep - 1, 1]),
frameon=False)
else:
veh_box = AnnotationBbox(oi, (history[-1, 0], history[-1, 1]),
frameon=False)
veh_box.zorder = 700
ax.add_artist(veh_box)
i += 1
else:
# ax.plot(history[:, 0], history[:, 1], 'k--')
if plot_prediction:
pis = predictions.pis
for component in range(
predictions.mixture_distribution.param_shape[-1]):
if pi_alpha:
line_alpha = pis[component].item()
ax.plot(
predictions.component_distribution.mean[at_timestep:,
0, component,
0] + x_min,
predictions.component_distribution.mean[at_timestep:,
0, component,
1] + y_min,
color=cmap[1],
linewidth=line_width,
alpha=line_alpha,
zorder=600)
if plot_future:
ax.plot(future[at_timestep:, 0],
future[at_timestep:, 1],
'w--',
zorder=650,
path_effects=[
pe.Stroke(linewidth=edge_width, foreground='k'),
pe.Normal()
])
# Current Node Position
if at_timestep > 0:
x, y = future[at_timestep - 1, 0], future[at_timestep - 1, 1]
else:
x, y = history[-1, 0], history[-1, 1]
circle = plt.Circle((x, y),
node_circle_size,
facecolor='g',
edgecolor='k',
lw=circle_edge_width,
zorder=3)
ax.add_artist(circle)
# Visualizing the ego-vehicle as well.
position_state = {'position': ['x', 'y']}
history = scene.robot.get(np.array([ts_key - max_hl, ts_key]),
position_state) # History includes current pos
history = history[~np.isnan(history.sum(axis=1))]
history += np.array([x_min, y_min])
future = scene.robot.get(np.array([ts_key + 1, ts_key + ph]),
position_state)
future = future[~np.isnan(future.sum(axis=1))]
future += np.array([x_min, y_min])
if plot_future:
ax.plot(
future[at_timestep:, 0],
future[at_timestep:, 1],
'w--o',
linewidth=4,
markersize=3,
zorder=650,
path_effects=[pe.Stroke(linewidth=5, foreground='k'),
pe.Normal()])
vel = scene.robot.get(np.array([ts_key + at_timestep]),
{'velocity': ['x', 'y']})
h = np.arctan2(vel[0, 1], vel[0, 0])
r_img = rotate(robot,
scene.robot.get(np.array([ts_key + at_timestep]),
{'heading': ['°']})[0, 0] * 180 / np.pi,
reshape=True)
oi = OffsetImage(r_img, zoom=car_img_zoom, zorder=700)
if at_timestep > 0:
veh_box = AnnotationBbox(
oi, (future[at_timestep - 1, 0], future[at_timestep - 1, 1]),
frameon=False)
else:
veh_box = AnnotationBbox(oi, (history[-1, 0], history[-1, 1]),
frameon=False)
veh_box.zorder = 700
ax.add_artist(veh_box)
def plot_vehicle_dist(ax,
predictions,
scene,
max_hl=10,
ph=6,
map=None,
x_min=0,
y_min=0,
pi_alpha=False,
line_alpha=0.8,
line_width=0.2,
edge_width=2,
circle_edge_width=0.5,
node_circle_size=0.3,
car_img_zoom=0.01,
pi_threshold=0.05):
prediction_dict, histories_dict, futures_dict = prediction_output_to_trajectories(
predictions, max_hl, ph, map=map)
assert (len(prediction_dict.keys()) <= 1)
if len(prediction_dict.keys()) == 0:
return
ts_key = list(prediction_dict.keys())[0]
prediction_dict = prediction_dict[ts_key]
histories_dict = histories_dict[ts_key]
futures_dict = futures_dict[ts_key]
if map is not None:
ax.imshow(map.fdata, origin='lower', alpha=0.5)
cmap = ['k', 'b', 'y', 'g', 'r']
a = []
i = 0
node_list = sorted(histories_dict.keys(), key=lambda x: x.id)
for node in node_list:
history = histories_dict[node] + np.array([x_min, y_min])
future = futures_dict[node] + np.array([x_min, y_min])
predictions = prediction_dict[node]
pis = predictions.pis
if node.type.name == 'VEHICLE':
ax.plot(future[:, 0],
future[:, 1],
'w--o',
linewidth=4,
markersize=3,
zorder=650,
path_effects=[
pe.Stroke(linewidth=5, foreground='k'),
pe.Normal()
])
for component in range(
predictions.mixture_distribution.param_shape[-1]):
pi = pis[component].item()
if pi < pi_threshold:
continue
means = predictions.component_distribution.mean[:, 0,
component, :
2] + np.array([
x_min,
y_min
])
covs = predictions.component_distribution.covariance_matrix[:,
0,
component, :
2, :
2]
ax.plot(means[..., 0],
means[..., 1],
'-o',
markersize=3,
color=cmap[1],
linewidth=line_width,
alpha=line_alpha,
zorder=620)
for timestep in range(means.shape[0]):
mean = means[timestep]
covar = covs[timestep]
v, w = linalg.eigh(covar)
v = 2. * np.sqrt(2.) * np.sqrt(v)
u = w[0] / linalg.norm(w[0])
# Plot an ellipse to show the Gaussian component
angle = np.arctan2(u[1], u[0])
angle = 180. * angle / np.pi # convert to degrees
ell = patches.Ellipse(mean,
v[0],
v[1],
180. + angle,
color='blue',
zorder=600)
ell.set_edgecolor(None)
ell.set_clip_box(ax.bbox)
ell.set_alpha(pi / 3.)
ax.add_artist(ell)
vel = node.get(np.array([ts_key]), {'velocity': ['x', 'y']})
h = np.arctan2(vel[0, 1], vel[0, 0])
r_img = rotate(
cars[i % len(cars)],
node.get(np.array([ts_key]), {'heading': ['°']})[0, 0] * 180 /
np.pi,
reshape=True)
oi = OffsetImage(r_img, zoom=car_img_zoom, zorder=700)
veh_box = AnnotationBbox(oi, (history[-1, 0], history[-1, 1]),
frameon=False)
veh_box.zorder = 700
ax.add_artist(veh_box)
i += 1
else:
for component in range(
predictions.mixture_distribution.param_shape[-1]):
pi = pis[component].item()
if pi < pi_threshold:
continue
means = predictions.component_distribution.mean[:, 0,
component, :
2] + np.array([
x_min,
y_min
])
covs = predictions.component_distribution.covariance_matrix[:,
0,
component, :
2, :
2]
ax.plot(means[..., 0],
means[..., 1],
'-o',
markersize=3,
color=cmap[1],
linewidth=line_width,
alpha=line_alpha,
zorder=620)
for timestep in range(means.shape[0]):
mean = means[timestep]
covar = covs[timestep]
v, w = linalg.eigh(covar)
v = 2. * np.sqrt(2.) * np.sqrt(v)
u = w[0] / linalg.norm(w[0])
# Plot an ellipse to show the Gaussian component
angle = np.arctan2(u[1], u[0])
angle = 180. * angle / np.pi # convert to degrees
ell = patches.Ellipse(mean,
v[0],
v[1],
180. + angle,
color='blue',
zorder=600)
ell.set_edgecolor(None)
ell.set_clip_box(ax.bbox)
ell.set_alpha(pi / 3.)
ax.add_artist(ell)
ax.plot(future[:, 0],
future[:, 1],
'w--',
zorder=650,
path_effects=[
pe.Stroke(linewidth=edge_width, foreground='k'),
pe.Normal()
])
# Current Node Position
circle = plt.Circle((history[-1, 0], history[-1, 1]),
node_circle_size,
facecolor='g',
edgecolor='k',
lw=circle_edge_width,
zorder=3)
ax.add_artist(circle)
# Visualizing the ego-vehicle as well.
position_state = {'position': ['x', 'y']}
history = scene.robot.get(np.array([ts_key - max_hl, ts_key]),
position_state) # History includes current pos
history = history[~np.isnan(history.sum(axis=1))]
history += np.array([x_min, y_min])
future = scene.robot.get(np.array([ts_key + 1, ts_key + ph]),
position_state)
future = future[~np.isnan(future.sum(axis=1))]
future += np.array([x_min, y_min])
ax.plot(future[:, 0],
future[:, 1],
'w--o',
linewidth=4,
markersize=3,
zorder=650,
path_effects=[pe.Stroke(linewidth=5, foreground='k'),
pe.Normal()])
vel = scene.robot.get(np.array([ts_key]), {'velocity': ['x', 'y']})
h = np.arctan2(vel[0, 1], vel[0, 0])
r_img = rotate(
robot,
scene.robot.get(np.array([ts_key]), {'heading': ['°']})[0, 0] * 180 /
np.pi,
reshape=True)
oi = OffsetImage(r_img, zoom=car_img_zoom, zorder=700)
veh_box = AnnotationBbox(oi, (history[-1, 0], history[-1, 1]),
frameon=False)
veh_box.zorder = 700
ax.add_artist(veh_box)
def compute_batch_statistics(prediction_output_dict,
dt,
max_hl,
ph,
node_type_enum,
kde=True,
obs=False,
map=None,
prune_ph_to_future=False,
best_of=False,
col=False):
(prediction_dict, _, futures_dict) = prediction_output_to_trajectories(
prediction_output_dict,
dt,
max_hl,
ph,
prune_ph_to_future=prune_ph_to_future)
batch_error_dict = dict()
for node_type in node_type_enum:
batch_error_dict[node_type] = {
'ade': list(),
'fde': list(),
'col_joint': list(),
'col_truth': list(),
'col_cross': list(),
'kde': list(),
'obs_viols': list()
}
for t in prediction_dict.keys():
if col:
prediction_joint = list()
futures_joint = list()
for node in prediction_dict[t].keys():
prediction_joint.append(prediction_dict[t][node][0, 0])
futures_joint.append(futures_dict[t][node])
prediction_joint = np.stack(prediction_joint, axis=0)
futures_joint = np.stack(futures_joint, axis=0)
for node in prediction_dict[t].keys():
ade_errors = compute_ade(prediction_dict[t][node],
futures_dict[t][node])
fde_errors = compute_fde(prediction_dict[t][node],
futures_dict[t][node])
if col:
idx_neighbors = abs(futures_joint[:, 0, 0] -
futures_dict[t][node][None, 0, 0]) > 1e-8
if idx_neighbors.sum() > 0:
num_interp = 4
col_joint = compute_col(
prediction_dict[t][node][0, 0],
prediction_joint[idx_neighbors],
num_interp=num_interp).astype(float)
col_cross = compute_col(
prediction_dict[t][node][0, 0],
futures_joint[idx_neighbors],
num_interp=num_interp).astype(float)
col_truth = compute_col(futures_dict[t][node],
futures_joint[idx_neighbors],
num_interp=num_interp)
col_joint[col_truth] = float('nan')
col_cross[col_truth] = float('nan')
col_truth = col_truth.astype(float)
if col_truth.any():
# skip frames where the groud truth observations lead to collisions
ade_errors[:] = float('nan')
fde_errors[:] = float('nan')
else:
col_joint = np.array([float('nan')] * (56))
col_truth = np.array([float('nan')] * (56))
col_cross = np.array([float('nan')] * (56))
else:
col_joint = 0
col_truth = 0
col_cross = 0
if kde:
kde_ll = compute_kde_nll(prediction_dict[t][node],
futures_dict[t][node])
else:
kde_ll = 0
if obs:
obs_viols = compute_obs_violations(prediction_dict[t][node],
map)
else:
obs_viols = 0
if best_of:
ade_errors = np.min(ade_errors, keepdims=True)
fde_errors = np.min(fde_errors, keepdims=True)
kde_ll = np.min(kde_ll)
batch_error_dict[node.type]['ade'].extend(list(ade_errors))
batch_error_dict[node.type]['fde'].extend(list(fde_errors))
batch_error_dict[node.type]['col_joint'].extend([col_joint])
batch_error_dict[node.type]['col_truth'].extend([col_truth])
batch_error_dict[node.type]['col_cross'].extend([col_cross])
batch_error_dict[node.type]['kde'].extend([kde_ll])
batch_error_dict[node.type]['obs_viols'].extend([obs_viols])
return batch_error_dict