def compute_trajectory(pose_vec, gt_traj, method='odom'):
est_traj = [gt_traj[0]]
cum_dist = [0]
for i in range(0, pose_vec.shape[0]):
#classically estimated traj
dT = SE3.exp(pose_vec[i])
new_est = SE3.as_matrix(
(dT.dot(SE3.from_matrix(est_traj[i], normalize=True).inv())).inv())
est_traj.append(new_est)
cum_dist.append(cum_dist[i] + np.linalg.norm(dT.trans))
gt_traj_se3 = [SE3.from_matrix(T, normalize=True) for T in gt_traj]
est_traj_se3 = [SE3.from_matrix(T, normalize=True) for T in est_traj]
tm_est = TrajectoryMetrics(gt_traj_se3, est_traj_se3, convention='Twv')
est_mean_trans, est_mean_rot = tm_est.mean_err()
est_mean_rot = (est_mean_rot * 180 / np.pi).round(3)
est_mean_trans = est_mean_trans.round(3)
seg_lengths = list(range(100, 801, 100))
_, seg_errs_est = tm_est.segment_errors(seg_lengths, rot_unit='rad')
print('trans. rel. err: {}, rot. rel. err: {}'.format(
np.mean(tm_est.rel_errors()[0]), np.mean(tm_est.rel_errors()[1])))
rot_seg_err = (100 * np.mean(seg_errs_est[:, 2]) * 180 / np.pi).round(3)
trans_seg_err = (np.mean(seg_errs_est[:, 1]) * 100).round(3)
if np.isnan(trans_seg_err):
max_dist = cum_dist[-1] - cum_dist[-1] % 100 + 1 - 100
print('max dist', max_dist)
seg_lengths = list(range(100, int(max_dist), 100))
_, seg_errs_est = tm_est.segment_errors(seg_lengths, rot_unit='rad')
rot_seg_err = (100 * np.mean(seg_errs_est[:, 2]) * 180 /
np.pi).round(3)
trans_seg_err = (np.mean(seg_errs_est[:, 1]) * 100).round(3)
print("{} mean trans. error: {} | mean rot. error: {}".format(
method, est_mean_trans, est_mean_rot))
print("{} mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format(
method, trans_seg_err, rot_seg_err))
errors = (est_mean_trans, est_mean_rot, trans_seg_err, rot_seg_err)
return np.array(est_traj), np.array(gt_traj), errors, np.array(cum_dist)
odom_pose_vec = matfile['odom_pose_vecs'].item(
) #original odometry 6x1 vecs (seq_size-1 x 6)
corr_pose_vec = matfile['corr_pose_vecs'].item(
) # corrected 6x1 vecs (seq_size-1 x 6)
if estimator == 'stereo': #use rotation corrections only.
corr_pose_vec[:, :, 0:3] = odom_pose_vec[:, 0:3]
best_loop_closure_pose_vec = corr_pose_vec[best_loop_closure_epoch]
est_traj, avg_corr_traj, dense_traj, dense_gt = [], [], [], []
est_traj.append(gt_traj[0])
avg_corr_traj.append(gt_traj[0])
for i in range(0, odom_pose_vec.shape[0]):
#classically estimated traj
dT = SE3.exp(odom_pose_vec[i])
new_est = SE3.as_matrix(
(dT.dot(SE3.from_matrix(est_traj[i], normalize=True).inv())).inv())
est_traj.append(new_est)
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='')
if estimator == 'stereo': #use rotation corrections only.
corr_pose_vec[:,:,0:3] = odom_pose_vec[:,0:3]
###Use the epoch with the lowest loss, or most loop closures:
best_loss_pose_vec = corr_pose_vec[best_loss_epoch]
best_loop_closure_pose_vec = corr_pose_vec[best_loop_closure_epoch]
est_traj, best_loss_traj, best_loop_closure_traj = [],[],[]
est_traj.append(gt_traj[0])
best_loss_traj.append(gt_traj[0])
best_loop_closure_traj.append(gt_traj[0])
for i in range(0,odom_pose_vec.shape[0]):
#classically estimated traj
dT = SE3.exp(odom_pose_vec[i])
new_est = SE3.as_matrix((dT.dot(SE3.from_matrix(est_traj[i], normalize=True).inv())).inv())
est_traj.append(new_est)
#best validation loss traj
dT_corr = SE3.exp(best_loss_pose_vec[i])
new_est = SE3.as_matrix((dT_corr.dot(SE3.from_matrix(best_loss_traj[i], normalize=True).inv())).inv())
best_loss_traj.append(new_est)
#best loop closure traj
dT_corr = SE3.exp(best_loop_closure_pose_vec[i])
new_est = SE3.as_matrix((dT_corr.dot(SE3.from_matrix(best_loop_closure_traj[i], normalize=True).inv())).inv())
best_loop_closure_traj.append(new_est)
est_tm, est_metrics = generate_trajectory_metrics(gt_traj, est_traj, name='libviso2', seq=seq, mode='---')
best_loss_tm, best_loss_metrics = generate_trajectory_metrics(gt_traj, best_loss_traj, name='Ours', seq='', mode='best loss')
best_loop_closure_tm, best_loop_closure_metrics = generate_trajectory_metrics(gt_traj, best_loop_closure_traj, name='Ours', seq='', mode='loop closure')
def test_trajectory(device, pose_model, spatial_trans, dset, epoch):
pose_model.train(False) # Set model to evaluate mode
pose_model.eval() #used for batch normalization # Set model to training mode
spatial_trans.train(False)
spatial_trans.eval()
#initialize the relevant outputs
full_corr_lie_alg_stacked, rot_corr_lie_alg_stacked, gt_lie_alg_stacked, vo_lie_alg_stacked, corrections_stacked, gt_corrections_stacked= \
np.empty((0,6)), np.empty((0,6)), np.empty((0,6)), np.empty((0,6)), np.empty((0,6)), np.empty((0,6))
for data in dset:
imgs, gt_lie_alg, intrinsics, vo_lie_alg, gt_correction = data
gt_lie_alg = gt_lie_alg.type(torch.FloatTensor).to(device)
vo_lie_alg = vo_lie_alg.type(torch.FloatTensor).to(device)
img_list = []
for im in imgs:
img_list.append(im.to(device))
corr, exp_mask, disp = pose_model(img_list[0:3], vo_lie_alg)
exp_mask, disp = exp_mask[0], disp[0][:,0]
corr_rot = torch.clone(corr)
corr_rot[:,0:3]=0
corrected_pose = se3_log_exp(corr, vo_lie_alg)
corrected_pose_rot_only = se3_log_exp(corr_rot, vo_lie_alg)
corrections_stacked = np.vstack((corrections_stacked, corr.cpu().detach().numpy()))
gt_corrections_stacked = np.vstack((gt_corrections_stacked, gt_correction.cpu().detach().numpy()))
full_corr_lie_alg_stacked = np.vstack((full_corr_lie_alg_stacked, corrected_pose.cpu().detach().numpy()))
rot_corr_lie_alg_stacked = np.vstack((rot_corr_lie_alg_stacked, corrected_pose_rot_only.cpu().detach().numpy()))
gt_lie_alg_stacked = np.vstack((gt_lie_alg_stacked, gt_lie_alg.cpu().detach().numpy()))
vo_lie_alg_stacked = np.vstack((vo_lie_alg_stacked, vo_lie_alg.cpu().detach().numpy()))
est_traj, corr_traj, corr_traj_rot, gt_traj = [],[],[],[]
gt_traj = dset.dataset.raw_gt_trials[0]
est_traj.append(gt_traj[0])
corr_traj.append(gt_traj[0])
corr_traj_rot.append(gt_traj[0])
cum_dist = [0]
for i in range(0,full_corr_lie_alg_stacked.shape[0]):
#classically estimated traj
dT = SE3.exp(vo_lie_alg_stacked[i])
new_est = SE3.as_matrix((dT.dot(SE3.from_matrix(est_traj[i],normalize=True).inv())).inv())
est_traj.append(new_est)
cum_dist.append(cum_dist[i]+np.linalg.norm(dT.trans))
#corrected traj (rotation only)
dT = SE3.exp(rot_corr_lie_alg_stacked[i])
new_est = SE3.as_matrix((dT.dot(SE3.from_matrix(corr_traj_rot[i],normalize=True).inv())).inv())
corr_traj_rot.append(new_est)
#
#
# #corrected traj (full pose)
dT = SE3.exp(full_corr_lie_alg_stacked[i])
new_est = SE3.as_matrix((dT.dot(SE3.from_matrix(corr_traj[i],normalize=True).inv())).inv())
corr_traj.append(new_est)
gt_traj_se3 = [SE3.from_matrix(T,normalize=True) for T in gt_traj]
est_traj_se3 = [SE3.from_matrix(T,normalize=True) for T in est_traj]
corr_traj_se3 = [SE3.from_matrix(T,normalize=True) for T in corr_traj]
corr_traj_rot_se3 = [SE3.from_matrix(T,normalize=True) for T in corr_traj_rot]
tm_est = TrajectoryMetrics(gt_traj_se3, est_traj_se3, convention = 'Twv')
tm_corr = TrajectoryMetrics(gt_traj_se3, corr_traj_se3, convention = 'Twv')
tm_corr_rot = TrajectoryMetrics(gt_traj_se3, corr_traj_rot_se3, convention = 'Twv')
if epoch >= 0:
est_mean_trans, est_mean_rot = tm_est.mean_err()
corr_mean_trans, corr_mean_rot = tm_corr.mean_err()
corr_rot_mean_trans, corr_rot_mean_rot = tm_corr_rot.mean_err()
print("Odom. mean trans. error: {} | mean rot. error: {}".format(est_mean_trans, est_mean_rot*180/np.pi))
print("Corr. mean trans. error: {} | mean rot. error: {}".format(corr_mean_trans, corr_mean_rot*180/np.pi))
print("Corr. (rot. only) mean trans. error: {} | mean rot. error: {}".format(corr_rot_mean_trans, corr_rot_mean_rot*180/np.pi))
seg_lengths = list(range(100,801,100))
_, seg_errs_est = tm_est.segment_errors(seg_lengths, rot_unit='rad')
_, seg_errs_corr = tm_corr.segment_errors(seg_lengths, rot_unit='rad')
_, seg_errs_corr_rot = tm_corr_rot.segment_errors(seg_lengths, rot_unit='rad')
print("Odom. mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format(np.mean(seg_errs_est[:,1])*100, 100*np.mean(seg_errs_est[:,2])*180/np.pi))
print("Corr. mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format(np.mean(seg_errs_corr[:,1])*100, 100*np.mean(seg_errs_corr[:,2])*180/np.pi))
print("Corr. (rot. only) mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format(np.mean(seg_errs_corr_rot[:,1])*100, 100*np.mean(seg_errs_corr_rot[:,2])*180/np.pi))
rot_seg_err = 100*np.mean(seg_errs_corr_rot[:,2])*180/np.pi
return corrections_stacked, gt_corrections_stacked, full_corr_lie_alg_stacked, vo_lie_alg_stacked, gt_lie_alg_stacked, \
np.array(corr_traj), np.array(corr_traj_rot), np.array(est_traj), np.array(gt_traj), rot_seg_err, corr_rot_mean_trans, np.array(cum_dist)
