def align_trajectory(self):
if self.data_aligned:
print("Trajectory already aligned")
return
print("Aliging the trajectory estimate to the groundtruth...")
print("Alignment type is {0}.".format(self.align_type))
n = int(self.align_num_frames)
if n < 0.0:
print('To align all frames.')
n = len(self.p_es)
else:
print('To align trajectory using ' + str(n) + ' frames.')
self.trans = np.zeros((3,))
self.rot = np.eye(3)
self.scale = 1.0
if self.align_type == 'none':
pass
else:
self.scale, self.rot, self.trans = au.alignTrajectory(
self.p_es, self.p_gt, self.q_es, self.q_gt,
self.align_type, self.align_num_frames)
self.p_es_aligned = np.zeros(np.shape(self.p_es))
self.q_es_aligned = np.zeros(np.shape(self.q_es))
for i in range(np.shape(self.p_es)[0]):
self.p_es_aligned[i, :] = self.scale * \
self.rot.dot(self.p_es[i, :]) + self.trans
q_es_R = self.rot.dot(
tf.quaternion_matrix(self.q_es[i, :])[0:3, 0:3])
q_es_T = np.identity(4)
q_es_T[0:3, 0:3] = q_es_R
self.q_es_aligned[i, :] = tf.quaternion_from_matrix(q_es_T)
self.data_aligned = True
print("... trajectory alignment done.")
print("Calculate angular and linear velocity...")
def align_trajectory(self):
if self.data_aligned:
print("Trajectory already aligned")
return
print(Fore.RED +
"Aliging the trajectory estimate to the groundtruth...")
print("Alignment type is {0}.".format(self.align_type))
n = int(self.align_num_frames)
if n < 0.0:
print('To align all frames.')
n = len(self.p_es)
else:
print('To align trajectory using ' + str(n) + ' frames.')
self.trans = np.zeros((3, ))
self.rot = np.eye(3)
self.scale = 1.0
if self.align_type == 'none':
pass
else:
self.scale, self.rot, self.trans = au.alignTrajectory(
self.p_es, self.p_gt, self.q_es, self.q_gt, self.align_type,
self.align_num_frames)
self.p_es_aligned = np.zeros(np.shape(self.p_es))
self.q_es_aligned = np.zeros(np.shape(self.q_es))
for i in range(np.shape(self.p_es)[0]):
self.p_es_aligned[i, :] = self.scale * \
self.rot.dot(self.p_es[i, :]) + self.trans
q_es_R = self.rot.dot(
tf.quaternion_matrix(self.q_es[i, :])[0:3, 0:3])
q_es_T = np.identity(4)
q_es_T[0:3, 0:3] = q_es_R
self.q_es_aligned[i, :] = tf.quaternion_from_matrix(q_es_T)
self.data_aligned = True
print(Fore.GREEN + "... trajectory alignment done:\n"
"trans:\n{}\n"
"rot:\n{}\n"
"scale:\n{}\n".format(self.trans, self.rot, self.scale))
print(Fore.GREEN + "... inverse transform:\n"
"trans:\n{}\n"
"rot:\n{}\n"
"scale:\n{}\n".format(-np.transpose(self.rot).dot(self.trans),
np.transpose(self.rot), self.scale))
tf_file = open(os.path.join(self.data_dir, 'transform.csv'), 'w')
# optimized_transform = np.append(self.rot, self.trans.transpose())
# np.savetxt(tf_file, optimized_transform, delimiter=",")
tf_file.write('# T0, T1, T2, T3\n')
tf_file.write("{0:.9f},{1:.9f},{2:.9f},{3:.9f}\n"
"{4:.9f},{5:.9f},{6:.9f},{7:.9f}\n"
"{8:.9f},{9:.9f},{10:.9f},{11:.9f}\n"
"0.0,0.0,0.0,1.0\n".format(
self.rot[0][0], self.rot[0, 1], self.rot[0, 2],
self.trans[0], self.rot[1][0], self.rot[1, 1],
self.rot[1, 2], self.trans[1], self.rot[2][0],
self.rot[2, 1], self.rot[2, 2], self.trans[2]))
