def integrate_quaternion_zero_order_hold(t_W_B0, v_W_B0, R_W_B0, B_a_W_B,
B_w_W_B, dt):
n = np.shape(B_a_W_B)[0]
t_W_B = np.zeros((n, 3))
t_W_B[0, :] = t_W_B0
v_W_B = np.zeros((n, 3))
v_W_B[0, :] = v_W_B0
R_W_B = np.zeros((n, 9))
R_W_B[0, :] = R_W_B0
for i in range(1, n):
# Last state:
R_W_Bk = np.reshape(R_W_B[i - 1, :], (3, 3))
#q_Bk_W = tf.quaternion_from_matrix(tf.convert_3x3_to_4x4(np.transpose(R_W_Bk)))
q_Bk_W = tf.quaternion_from_matrix(tf.convert_3x3_to_4x4(R_W_Bk))
a = B_a_W_B[i - 1, :]
w = B_w_W_B[i - 1, :]
Theta = np.eye(4) + 0.5 * dt * tf.quat_Omega(
w) # Eq. 124 Quat. Tutorial
q_Bkp1_W = np.dot(Theta, q_Bk_W) # Eq. 111 Quat. Tutorial
#R_W_B[i,:] = np.reshape(np.transpose(tf.matrix_from_quaternion(q_Bkp1_W)[:3,:3]), (9,))
R_W_B[i, :] = np.reshape(
tf.matrix_from_quaternion(q_Bkp1_W)[:3, :3], (9, ))
v_W_B[i, :] = v_W_B[i - 1, :] + g * dt + np.dot(R_W_Bk, a) * dt
t_W_B[i, :] = t_W_B[i - 1, :] + v_W_B[i - 1, :] * dt
return R_W_B, v_W_B, t_W_B
def state_derivative(imu_acc, imu_gyr, state, dt):
q_W_B = state[:4]
v = state[4:7]
R_W_B = tf.matrix_from_quaternion(q_W_B)[:3,:3]
Omega = tf.quat_Omega(imu_gyr)
# Quaternion derivative according to MARS-Lab Quaternion Tutorial.
q_dot = 0.5 * np.dot(Omega, q_W_B)
v_dot = np.array([0.0, 0.0, -gravity]) + np.dot(R_W_B, imu_acc)
p_dot = v # + dt * np.array([0.0, 0.0, -gravity]) + dt * np.dot(R_W_B, imu_acc)
state_dot = np.concatenate((q_dot, v_dot, p_dot))
return state_dot
def compute_rms_errors(self):
"""Compute Root Mean Square Error (RMSE) of aligned trajectory w.r.t
groundtruth trajectory.
"""
if not self.data_aligned:
raise ValueError("You need to first load and align the data")
# position error
e_trans = (self.p_gt - self.p_es_aligned)
e_trans_euclidean = np.sqrt(np.sum(e_trans**2, 1))
self.logger.info('Compute orientation error')
e_rot = np.zeros((len(e_trans_euclidean, )))
e_rpy = np.zeros(np.shape(self.p_es_aligned))
for i in range(np.shape(self.p_es_aligned)[0]):
R_we = tf.matrix_from_quaternion(self.q_es_aligned[i, :])
R_wg = tf.matrix_from_quaternion(self.q_gt[i, :])
R_ge = np.dot(np.linalg.inv(R_wg), R_we)
e_rpy[i, :] = tf.euler_from_matrix(R_ge, 'rzyx')
e_rot[i] = np.linalg.norm(tf.logmap_so3(R_ge[:3, :3]))
self.logger.info('Compute scale drift')
motion_gt = np.diff(self.p_gt, 0)
motion_es = np.diff(self.p_es_aligned, 0)
dist_gt = np.sqrt(np.sum(np.multiply(motion_gt, motion_gt), 1))
dist_es = np.sqrt(np.sum(np.multiply(motion_es, motion_es), 1))
e_scale_rel = np.divide(dist_es, dist_gt) - 1.0
self.logger.info('Save error plots')
traj_plot.plot_translation_error(self.distances, e_trans,
self.result_dir)
traj_plot.plot_rotation_error(self.distances, e_rpy, self.result_dir)
traj_plot.plot_scale_error(self.distances, e_scale_rel * 100.0,
self.result_dir)
# compute error statistics:
self.compute_and_save_statistics(e_trans_euclidean, 'trans')
self.compute_and_save_statistics(e_rot, 'rot')
self.compute_and_save_statistics(e_scale_rel, 'scale')
def apply_hand_eye_calibration_to_groundtruth(self):
if not self.data_loaded:
raise ValueError("You need to first load the data")
self.logger.info('Apply hand-eye calibration to groundtruth data.')
self.T_B_V = traj_loading.load_hand_eye_calib_from_file(
os.path.join(self.result_dir, "dataset.yaml"))
self.T_V_B = tf.inverse_matrix(self.T_B_V)
n = np.shape(self.p_gt)[0]
for i in range(n):
T_W_V = tf.matrix_from_quaternion(self.q_gt[i, :])
T_W_V[:3, 3] = self.p_gt[i, :]
T_W_B = np.dot(T_W_V, self.T_V_B)
self.q_gt[i, :] = tf.quaternion_from_matrix(T_W_B)
self.p_gt[i, :] = T_W_B[:3, 3]
def align_trajectory(self, align_type='se3', first_idx=0, last_idx=-1):
"""Align trajectory segment with ground-truth trajectory.
first_idx: First index of data to align.
last_idx: Last index of data to align.
align_type: 'se3' - translation and orientation
'sim3' - translation, orientation, and scale
'identity' - no alignment
'first_frame' - align just the first frame
"""
if not self.data_loaded:
raise ValueError("You need to first load the data")
if last_idx < 0:
if first_idx == 0:
self.logger.info('Align trajectory using all frames.')
else:
self.logger.info('Align trajectory from index ' +
str(first_idx) + ' to the end.')
last_idx = len(self.p_es)
else:
self.logger.info('Align trajectory from index ' + str(first_idx) + \
' to ' + str(first_idx) + '.')
# Compute alignment parameters
if align_type == 'sim3':
self.logger.info('Align Sim3 - rotation, translation and scale.')
self.scale, self.R_Wgt_Wes, self.t_Wgt_Wes = \
align_trajectory.align_sim3(self.p_gt[first_idx:last_idx,:],
self.p_es[first_idx:last_idx,:])
elif align_type == 'se3':
self.logger.info('Align SE3 - rotation and translation.')
self.R_Wgt_Wes, self.t_Wgt_Wes = \
align_trajectory.align_se3(self.p_gt[first_idx:last_idx,:],
self.p_es[first_idx:last_idx,:])
self.scale = 1.0
elif align_type == 'identity':
self.logger.info('Align Identity.')
self.t_Wgt_Wes = np.zeros((3, ))
self.R_Wgt_Wes = np.eye(3)
self.scale = 1.0
elif align_type == 'first_frame':
self.logger.info('Align first frame.')
T_Wgt_B = tf.matrix_from_quaternion(self.q_gt[0, :])
T_Wgt_B[:3, 3] = self.p_gt[0, :]
T_Wes_B = tf.matrix_from_quaternion(self.q_es[0, :])
T_Wes_B[:3, 3] = self.p_es[0, :]
T_Wgt_Wes = np.dot(T_Wgt_B, tf.inverse_matrix(T_Wes_B))
self.t_Wgt_Wes = T_Wgt_Wes[:3, 3]
self.R_Wgt_Wes = T_Wgt_Wes[:3, :3]
self.scale = 1.0
self.logger.info('Alignment translation t_Wgt_Wes: \n' +
str(self.t_Wgt_Wes))
self.logger.info('Alignment rotation R_Wgt_Wes: \n' +
str(self.R_Wgt_Wes))
self.logger.info('Alignment scale: \n' + str(self.scale))
npt.assert_almost_equal(np.linalg.det(self.R_Wgt_Wes), 1.0)
self.logger.info(
'Apply alignment to estimated trajectory to fit groundtruth')
q = tf.quaternion_from_matrix(tf.convert_3x3_to_4x4(self.R_Wgt_Wes))
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 * np.dot(self.R_Wgt_Wes, self.p_es[i,:]) \
+ self.t_Wgt_Wes
self.q_es_aligned[i, :] = tf.quaternion_multiply(
q, self.q_es[i, :])
self.align_first_idx = first_idx
self.align_last_idx = last_idx
self.data_aligned = True
def integrate_quaternion_runge_kutta_4(t_W_B0, v_W_B0, R_W_B0, B_a_W_B, B_w_W_B, dt):
n = np.shape(B_a_W_B)[0]
t_W_B = np.zeros((n,3))from pylab import setp
t_W_B[0,:] = t_W_B0
v_W_B = np.zeros((n,3))
v_W_B[0,:] = v_W_B0
R_W_B = np.zeros((n, 9))
R_W_B[0,:] = R_W_B0
def state_derivative(imu_acc, imu_gyr, state, dt):
q_W_B = state[:4]
v = state[4:7]
R_W_B = tf.matrix_from_quaternion(q_W_B)[:3,:3]
Omega = tf.quat_Omega(imu_gyr)
# Quaternion derivative according to MARS-Lab Quaternion Tutorial.
q_dot = 0.5 * np.dot(Omega, q_W_B)
v_dot = np.array([0.0, 0.0, -gravity]) + np.dot(R_W_B, imu_acc)
p_dot = v # + dt * np.array([0.0, 0.0, -gravity]) + dt * np.dot(R_W_B, imu_acc)
state_dot = np.concatenate((q_dot, v_dot, p_dot))
return state_dot
for i in range(1,n):
# Last state:
R = np.reshape(R_W_B[i-1,:], (3,3))
q_W_B = tf.quaternion_from_matrix(tf.convert_3x3_to_4x4(R))
v = v_W_B[i-1,:]
p = t_W_B[i-1,:]
# Get imu measurements from last, current timestamp and interpolate in between.
imu_acc_1 = B_a_W_B[i-1,:]
imu_acc_3 = B_a_W_B[i,:]
imu_acc_2 = 0.5 * (imu_acc_1 + imu_acc_3)
imu_gyr_1 = B_w_W_B[i-1,:]
imu_gyr_3 = B_w_W_B[i,:]
imu_gyr_2 = 0.5 * (imu_gyr_1 + imu_gyr_3)
# Runge-Kutta Integration:
state_1 = np.concatenate((q_W_B, v, p))
state_1_dot = state_derivative(imu_acc_1, imu_gyr_1, state_1, 0.0)
state_2 = state_1 + 0.5 * dt * state_1_dot
state_2 = renormalize_quaternion_in_state(state_2)
state_2_dot = state_derivative(imu_acc_2, imu_gyr_2, state_2, 0.5 * dt)
state_3 = state_1 + 0.5 * dt * state_2_dot
state_3 = renormalize_quaternion_in_state(state_3)
state_3_dot = state_derivative(imu_acc_2, imu_gyr_2, state_3, 0.5 * dt)
state_4 = state_1 + 1.0 * dt * state_3_dot
state_4 = renormalize_quaternion_in_state(state_4)
state_4_dot = state_derivative(imu_acc_3, imu_gyr_3, state_4, dt)
integrated_state = \
state_1 + 1.0 / 6.0 * dt * (state_1_dot + 2.0 * state_2_dot + 2.0 * state_3_dot + state_4_dot)
integrated_state = renormalize_quaternion_in_state(integrated_state)
# Save next state:
R = tf.matrix_from_quaternion(integrated_state[:4])[:3,:3]
R_W_B[i,:] = np.reshape(R, (9,))
v_W_B[i,:] = integrated_state[4:7]
t_W_B[i,:] = integrated_state[7:10]
return R_W_B, v_W_B, t_W_B