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