def generate_trajectory_metrics(gt_traj, est_traj, name='',seq='', mode=''): 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 = TrajectoryMetrics(gt_traj_se3, est_traj_se3, convention = 'Twv') est_mATE_trans, est_mATE_rot = tm.mean_err() est_mATE_rot = est_mATE_rot*180/np.pi print("{} ({}) mean trans. error: {} | mean rot. error: {}".format(name, mode, est_mATE_trans, est_mATE_rot)) seg_lengths = list(range(100,801,100)) _, est_seg_errs = tm.segment_errors(seg_lengths, rot_unit='rad') est_seg_err_trans = np.mean(est_seg_errs[:,1])*100 est_seg_err_rot = 100*np.mean(est_seg_errs[:,2])*180/np.pi print("{} ({}) mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format(name, mode, est_seg_err_trans, est_seg_err_rot) ) return tm, (seq, name, mode, est_mATE_trans.round(3), est_mATE_rot.round(3), est_seg_err_trans.round(3), est_seg_err_rot.round(3))
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)
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)