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