## Scale Factor
gt_norm = np.linalg.norm(gt_pose_vec[:, 0:3], axis=1)
plt.figure()
plt.grid()
plt.plot((cam_height / d)[plot_range])
plt.ylim([0.6, 2.0])
plt.title('Estimated Scale Factor Seq. {}'.format(
config['test_seq'][0].replace('_', '-')))
plt.savefig('{}/{}seq-{}-scale-vs-gt.png'.format(results_dir, prefix,
config['test_seq'][0]))
## Compute Trajectories
gt_traj = test_dset_loaders.dataset.raw_gt_trials[0]
orig_est, gt, errors, cum_dist = tt(unscaled_pose_vec,
gt_traj,
method='unscaled')
logger.log(seq, 'unscaled', errors[0], errors[1], errors[2], errors[3])
scaled_est, gt, errors, cum_dist = tt(scaled_pose_vec,
gt_traj,
method='scaled')
logger.log(seq, 'ground plane scaled', errors[0], errors[1], errors[2],
errors[3])
logger.log('', '', '', '', '', '')
vo_est, _, _, _ = tt(vo_pose_vec, gt_traj, method='vo')
## Plot trajectories
plt.figure()
plt.grid()
plt.plot(gt[:, 0, 3],
pose_norm = np.linalg.norm(unscaled_pose_vec[:,0:3],axis=1)
gt_norm = np.linalg.norm(gt_pose_vec[:,0:3],axis=1)
vo_norm = np.linalg.norm(vo_pose_vec[:,0:3],axis=1)
vo_scale_factor = np.average(gt_norm/vo_norm)
unscaled_pose_vec = unscaled_pose_vec[:,0:6]
scaled_pose_vec = np.array(unscaled_pose_vec)
scaled_pose_vec[:,0:3] = scaled_pose_vec[:,0:3]*np.repeat(cam_height/d.reshape((-1,1)),3,axis=1)
vo_pose_vec[:,0:3] = vo_pose_vec[:,0:3]*vo_scale_factor
## Compute Trajectories
gt_traj = test_dset_loaders.dataset.raw_gt_trials[0]
if method == method_list[0]:
vo_est, gt, errors, _ = tt(vo_pose_vec, gt_traj, method=method)
if method == method_list[1] or method == method_list[3]:
est, gt, errors, cum_dist = tt(unscaled_pose_vec,gt_traj,method=method)
if method == method_list[2]:
scaled_est, gt, errors, cum_dist = tt(scaled_pose_vec,gt_traj, method=method)
seq_results.append(errors[2])
errors = np.array(seq_results)
mean = np.mean(errors)
seq_results.append(mean)
seq_results = ["%.2f" % e for e in seq_results]
writer.writerow([method] + seq_results)
average_d = np.average(d)
unscaled_pose_vec = fwd_pose_vec1
# unscaled_pose_vec = -inv_pose_vec1
# unscaled_pose_vec = (fwd_pose_vec1 - inv_pose_vec1)/2
unscaled_pose_vec[:, 3:6] = gt_pose_vec[:, 3:6]
## align with global scale
gt_norm = np.linalg.norm(gt_pose_vec[:, 0:3], axis=1)
vo_norm = np.linalg.norm(vo_pose_vec[:, 0:3], axis=1)
vo_scale_factor = np.average(gt_norm / vo_norm)
unscaled_pose_vec = unscaled_pose_vec[:, 0:6]
scaled_pose_vec = np.array(unscaled_pose_vec)
scaled_pose_vec[:, 0:3] = scaled_pose_vec[:, 0:3] * np.repeat(
cam_height / d.reshape((-1, 1)), 3, axis=1)
vo_pose_vec[:, 0:3] = vo_pose_vec[:, 0:3] * vo_scale_factor
## Compute Trajectories
gt_traj = test_dset_loaders.dataset.raw_gt_trials[0]
est, gt, errors, cum_dist = tt(unscaled_pose_vec,
gt_traj,
method=method)
seq_results.append(errors[2])
errors = np.array(seq_results)
mean = np.mean(errors)
seq_results.append(mean)
seq_results = ["%.2f" % e for e in seq_results]
writer.writerow([method] + seq_results)
if config['dpc'] == True:
prefix = prefix = 'dpc-'
unscaled_pose_vec = fwd_pose_vec1
if use_gt_rot == True:
unscaled_pose_vec[:, 3:6] = gt_pose_vec[:, 3:6]
scaled_pose_vec = np.array(unscaled_pose_vec)
scaled_pose_vec[:, 0:3] = scaled_pose_vec[:, 0:3] * np.repeat(
data['dnet_scale_factor'], 3, axis=1)
## Compute Trajectories
gt_traj = test_dset_loaders.dataset.raw_gt_trials[0]
est, gt, errors, cum_dist = tt(unscaled_pose_vec,
gt_traj,
method='unscaled')
scaled_est, gt, errors, cum_dist = tt(scaled_pose_vec,
gt_traj,
method='scaled')
gt_traj_se3 = [SE3.from_matrix(T, normalize=True) for T in gt_traj]
est_se3 = [SE3.from_matrix(T, normalize=True) for T in est]
scaled_se3 = [
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')
num_workers=6)
ts = test_dset.raw_ts[0]
data = load_obj('{}{}/results/depth_opt_bottleneck/{}_results'.format(
path_to_ws, model_dir, seq))
# data = load_obj('{}{}/results/scale/{}_plane_fit'.format(path_to_ws, model_dir, seq))
poses = data['fwd_pose_vec_opt']
gt_pose_vec = data['gt_pose_vec']
# print(poses[1])
# print(gt_pose_vec[1])
gt_traj = test_dset_loaders.dataset.raw_gt_trials[0]
est_traj, gt_traj, _, _ = tt(poses, gt_traj, method='')
for i in range(0, poses.shape[0] - 4):
pose_window = np.copy(poses[i:i + 4])
init_pose = np.eye(4)
this_pose = np.eye(4)
ts_window = ts[i:i + 5]
filename = '%.6d.txt' % i
# print(filename)
out_file = '{}/{}'.format(out_folder, filename)
with open(out_file, 'w') as f:
f.write('%f %f %f %f %f %f %f %f\n' %
(ts_window[0], 0, 0, 0, 0, 0, 0, 1))
for j in range(0, pose_window.shape[0]):
this_pose = np.dot(this_pose, pose_vec_to_mat(pose_window[j]))
gt_norm = np.linalg.norm(gt_pose_vec[:, 0:3], axis=1)
plt.figure()
plt.grid()
if ransac_rescaling == True:
plt.plot((cam_height / d)[plot_range], label='ransac')
if plane_rescaling == True:
plt.plot(data['learned_scale_factor'], label='learned')
plt.ylim([0.6, 2.0])
plt.legend()
plt.title('Estimated Scale Factor Seq. {}'.format(seq.replace('_', '-')))
plt.savefig('{}/seq-{}-scale-vs-gt.png'.format(results_dir, seq))
## Compute Trajectories
gt_traj = test_dset_loaders.dataset.raw_gt_trials[0]
orig_est, gt, errors, cum_dist = tt(unscaled_pose_vec,
gt_traj,
method='unscaled')
logger.log(seq, 'unscaled', errors[0], errors[1], errors[2], errors[3])
if ransac_rescaling == True:
scaled_est, gt, errors, cum_dist = tt(scaled_pose_vec_ransac,
gt_traj,
method='scaled (ransac)')
logger.log(seq, 'ransac scaled', errors[0], errors[1], errors[2],
errors[3])
if plane_rescaling == True:
scaled_est_learned, _, errors, _ = tt(scaled_pose_vec_learned,
gt_traj,
method='scaled (learned)')
logger.log(seq, 'plane scaled', errors[0], errors[1], errors[2],
errors[3])
if dnet_rescaling == True:
