def plot_trajectory(self, FORMAT='.png'): print(">>> Plotting trajectory...") # trajectory view from top fig = plt.figure(figsize=(6, 5.5)) ax = fig.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='y [m]') pu.plot_trajectory_top(ax, self.p_es_aligned, 'b', 'Estimate') pu.plot_trajectory_top(ax, self.p_gt, 'm', 'Groundtruth') pu.plot_aligned_top(ax, self.p_es_aligned, self.p_gt, self.align_num_frames) plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) fig.tight_layout() fig.savefig(self.plots_dir + '/trajectory_top' + '_' + self.align_str + FORMAT, bbox_inches="tight") # trajectory view from side fig = plt.figure(figsize=(6, 5.5)) ax = fig.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='y [m]') pu.plot_trajectory_side(ax, self.p_es_aligned, 'b', 'Estimate') pu.plot_trajectory_side(ax, self.p_gt, 'm', 'Groundtruth') plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) fig.tight_layout() fig.savefig(self.plots_dir + '/trajectory_side' + '_' + self.align_str + FORMAT, bbox_inches="tight")
# compute the errors print(">>> Calculating errors...") traj = Trajectory(args.result_dir) traj.compute_absolute_error() traj.compute_relative_errors() traj.cache_current_error() traj.write_errors_to_yaml() # do some plotting print(">>> Plotting absolute error...") fig = plt.figure(figsize=(6, 5.5)) ax = fig.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='y [m]') pu.plot_trajectory_top(ax, traj.p_es_aligned, 'b', 'Estimate') pu.plot_trajectory_top(ax, traj.p_gt, 'm', 'Groundtruth') pu.plot_aligned_top(ax, traj.p_es_aligned, traj.p_gt, traj.align_num_frames) plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) fig.tight_layout() fig.savefig(plots_dir + '/trajectory_top' + '_' + traj.align_str + FORMAT, bbox_inches="tight") fig = plt.figure(figsize=(6, 5.5)) ax = fig.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='y [m]') pu.plot_trajectory_side(ax, traj.p_es_aligned, 'b', 'Estimate') pu.plot_trajectory_side(ax, traj.p_gt, 'm', 'Groundtruth') plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) fig.tight_layout() fig.savefig(plots_dir + '/trajectory_side' + '_' + traj.align_str + FORMAT, bbox_inches="tight") fig = plt.figure(figsize=(8, 2.5))
def plot_trajectories(dataset_trajectories_list, dataset_names, algorithm_names, datasets_out_dir, plot_settings, plot_idx=0, plot_side=True, plot_aligned=True, plot_traj_per_alg=True): for dataset_idx, dataset_nm in enumerate(dataset_names): output_dir = datasets_out_dir[dataset_nm] dataset_trajs = dataset_trajectories_list[dataset_idx] p_es_0 = {} p_gt_raw = (dataset_trajs[0])[plot_idx].p_gt_raw p_gt_0 = {} for traj_list in dataset_trajs: p_es_0[traj_list[plot_idx].alg] = traj_list[plot_idx].p_es_aligned p_gt_0[traj_list[plot_idx].alg] = traj_list[plot_idx].p_gt print("Collected trajectories to plot: {0}".format(algorithm_names)) assert sorted(algorithm_names) == sorted(list(p_es_0.keys())) print("Plotting {0}...".format(dataset_nm)) # plot trajectory fig = plt.figure(figsize=(6, 5.5)) ax = fig.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='y [m]') if dataset_nm in plot_settings['datasets_titles']: ax.set_title(plot_settings['datasets_titles'][dataset_nm]) for alg in algorithm_names: if plot_traj_per_alg: fig_i = plt.figure(figsize=(6, 5.5)) ax_i = fig_i.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='y [m]') pu.plot_trajectory_top( ax_i, p_es_0[alg], 'b', 'Estimate ' + plot_settings['algo_labels'][alg], 0.5) pu.plot_trajectory_top(ax_i, p_gt_0[alg], 'm', 'Groundtruth') if plot_aligned: pu.plot_aligned_top(ax_i, p_es_0[alg], p_gt_0[alg], -1) plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) fig_i.tight_layout() fig_i.savefig(output_dir + '/' + dataset_nm + '_trajectory_top_' + plot_settings['algo_labels'][alg] + FORMAT, bbox_inches="tight", dpi=args.dpi) plt.close(fig_i) pu.plot_trajectory_top(ax, p_es_0[alg], plot_settings['algo_colors'][alg], plot_settings['algo_labels'][alg]) plt.sca(ax) pu.plot_trajectory_top(ax, p_gt_raw, 'm', 'Groundtruth') plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) fig.tight_layout() fig.savefig(output_dir + '/' + dataset_nm + '_trajectory_top' + FORMAT, bbox_inches="tight", dpi=args.dpi) plt.close(fig) # plot trajectory side if not plot_side: continue fig = plt.figure(figsize=(6, 2.2)) ax = fig.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='z [m]') if dataset_nm in plot_settings['datasets_titles']: ax.set_title(plot_settings['datasets_titles'][dataset_nm]) for alg in algorithm_names: if plot_traj_per_alg: fig_i = plt.figure(figsize=(6, 5.5)) ax_i = fig_i.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='y [m]') pu.plot_trajectory_side( ax_i, p_es_0[alg], 'b', 'Estimate ' + plot_settings['algo_labels'][alg], 0.5) pu.plot_trajectory_side(ax_i, p_gt_0[alg], 'm', 'Groundtruth') plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) fig_i.tight_layout() fig_i.savefig(output_dir + '/' + dataset_nm + '_trajectory_side_' + plot_settings['algo_labels'][alg] + FORMAT, bbox_inches="tight", dpi=args.dpi) plt.close(fig_i) pu.plot_trajectory_side(ax, p_es_0[alg], plot_settings['algo_colors'][alg], plot_settings['algo_labels'][alg]) plt.sca(ax) pu.plot_trajectory_side(ax, p_gt_raw, 'm', 'Groundtruth') plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.) fig.tight_layout() fig.savefig(output_dir + '/' + dataset_nm + '_trajectory_side' + FORMAT, bbox_inches="tight", dpi=args.dpi) plt.close(fig)