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)