dT_corr = SE3.exp(best_loop_closure_pose_vec[i])
new_est = SE3.as_matrix((dT_corr.dot(
SE3.from_matrix(avg_corr_traj[i], normalize=True).inv())).inv())
avg_corr_traj.append(new_est)
est_tm, est_metrics = generate_trajectory_metrics(gt_traj,
est_traj,
name='libviso2',
seq=seq)
corr_tm, avg_corr_metrics = generate_trajectory_metrics(gt_traj,
avg_corr_traj,
name='ss-dpcnet',
seq='')
saved_traj[seq]['ours'] = corr_tm
saved_traj[seq]['libviso2'] = est_tm
for seq in ['09', '10']:
tm_dict = {
'Libviso2-m': saved_traj[seq]['libviso2'],
'Ours (Corrected)': saved_traj[seq]['ours'],
}
order_of_keys = ["Libviso2-m", "Ours (Corrected)"]
list_of_tuples = [(key, tm_dict[key]) for key in order_of_keys]
tm_dict = OrderedDict(list_of_tuples)
est_vis = visualizers.TrajectoryVisualizer(tm_dict)
plt.figure()
fig, ax = est_vis.plot_topdown(which_plane='xy',
outfile='figs/mono-seq-{}.pdf'.format(seq),
title=r'{}'.format(seq))
SE3.from_matrix(T, normalize=True) for T in scaled_est
]
est_tm = TrajectoryMetrics(gt_traj_se3, est_se3, convention='Twv')
scaled_tm = TrajectoryMetrics(gt_traj_se3,
scaled_se3,
convention='Twv')
tm_dict[method] = {
'unscaled': est_tm,
'scaled': scaled_tm,
}
plotting_dict = {
'Ours': tm_dict['scaled']['unscaled'],
'DNet': tm_dict['unscaled']['scaled']
}
est_vis = visualizers.TrajectoryVisualizer(plotting_dict)
plt.figure()
if use_gt_rot == True:
fig, ax = est_vis.plot_topdown(
which_plane='xz',
outfile='figures/test-seq-{}_gt_rot.pdf'.format(seq),
title=r'{}'.format(seq))
else:
fig, ax = est_vis.plot_topdown(
which_plane='xz',
outfile='figures/test-seq-{}.pdf'.format(seq),
title=r'{}'.format(seq))