def plot_trajectories(dataset_trajectories_dict, dataset_names, output_dir):
for dataset_idx, dataset_nm in enumerate(dataset_names):
dataset_trajs = dataset_trajectories_dict[dataset_idx]
p_es_0 = {}
p_gt_0 = dataset_trajs[0].p_gt
for traj in dataset_trajs:
p_es_0[traj.alg] = traj.p_es_aligned
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]')
for alg in p_es_0:
pu.plot_trajectory_top(ax, p_es_0[alg], COLORS[alg],
PLOT_LABELS[alg])
pu.plot_trajectory_top(ax, p_gt_0, '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")
plt.close(fig)
# plot trajectory side
fig = plt.figure(figsize=(6, 2.2))
ax = fig.add_subplot(111,
aspect='equal',
xlabel='x [m]',
ylabel='z [m]')
for alg in p_es_0:
pu.plot_trajectory_side(ax, p_es_0[alg], COLORS[alg],
PLOT_LABELS[alg])
pu.plot_trajectory_side(ax, p_gt_0, '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")
plt.close(fig)
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")
# 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))
ax = fig.add_subplot(111,
xlabel='Distance [m]',
ylabel='Position Drift [mm]',
xlim=[0, traj.accum_distances[-1]])
pu.plot_error_n_dim(ax, traj.accum_distances,
traj.abs_errors['abs_e_trans_vec'] * 1000, plots_dir)
ax.legend()
fig.tight_layout()
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)
