def shift0(traj):
'''
Traj: a list of [t + quat]
Return: translate and rotate the traj
'''
traj_ses = tf.pos_quats2SE_matrices(np.array(traj))
traj_init = traj_ses[0]
traj_init_inv = np.linalg.inv(traj_init)
new_traj = []
for tt in traj_ses:
ttt=traj_init_inv.dot(tt)
new_traj.append(tf.SE2pos_quat(ttt))
return np.array(new_traj)
def ned2cam(traj):
'''
transfer a ned traj to camera frame traj
'''
T = np.array([[0, 1, 0, 0], [0, 0, 1, 0], [1, 0, 0, 0], [0, 0, 0, 1]],
dtype=np.float32)
T_inv = np.linalg.inv(T)
new_traj = []
traj_ses = tf.pos_quats2SE_matrices(np.array(traj))
for tt in traj_ses:
ttt = T.dot(tt).dot(T_inv)
new_traj.append(tf.SE2pos_quat(ttt))
return np.array(new_traj)
def evaluate(self, gt_traj, est_traj, scale):
gt_xyz = np.matrix(gt_traj[:, 0:3].transpose())
est_xyz = np.matrix(est_traj[:, 0:3].transpose())
rot, trans, trans_error, s = align(gt_xyz, est_xyz, scale)
print(' ATE scale: {}'.format(s))
error = np.sqrt(np.dot(trans_error, trans_error) / len(trans_error))
# align two trajs
est_SEs = pos_quats2SE_matrices(est_traj)
T = np.eye(4)
T[:3, :3] = rot
T[:3, 3:] = trans
T = np.linalg.inv(T)
est_traj_aligned = []
for se in est_SEs:
se[:3, 3] = se[:3, 3] * s
se_new = T.dot(se)
se_new = SE2pos_quat(se_new)
est_traj_aligned.append(se_new)
return error, gt_traj, est_traj_aligned
def quats2SEs(gt_traj, est_traj):
gt_SEs = pos_quats2SE_matrices(gt_traj)
est_SEs = pos_quats2SE_matrices(est_traj)
return gt_SEs, est_SEs
