def compute_orientation_mean_and_rmse_relative(q_AB_list, q_AC_list):
assert q_AB_list.shape == q_AC_list.shape
assert q_AC_list.shape[1] == 4
orientation_errors_rad = np.zeros(q_AB_list.shape[0] - 1)
for idx in range(q_AB_list.shape[0] - 1):
q_ABk = Quaternion(q_AB_list[idx, 0], q_AB_list[idx, 1],
q_AB_list[idx, 2], q_AB_list[idx, 3])
q_ACk = Quaternion(q_AC_list[idx, 0], q_AC_list[idx, 1],
q_AC_list[idx, 2], q_AC_list[idx, 3])
q_ABkp1 = Quaternion(q_AB_list[idx + 1, 0], q_AB_list[idx + 1, 1],
q_AB_list[idx + 1, 2], q_AB_list[idx + 1, 3])
q_ACkp1 = Quaternion(q_AC_list[idx + 1, 0], q_AC_list[idx + 1, 1],
q_AC_list[idx + 1, 2], q_AC_list[idx + 1, 3])
q_Bk_Bkp1 = q_ABk.inverse() * q_ABkp1
q_Ck_Ckp1 = q_ACk.inverse() * q_ACkp1
q_error = q_Bk_Bkp1.inverse() * q_Ck_Ckp1
angle_error = q_error.angle_axis()[3]
if angle_error > np.pi:
angle_error -= 2 * np.pi
assert angle_error <= np.pi
assert angle_error >= -np.pi
orientation_errors_rad[idx] = np.abs(angle_error)
return ErrorStatistics(orientation_errors_rad)
def compute_orientation_mean_and_rmse_absolute(q_AB_list, q_AC_list):
assert q_AB_list.shape == q_AC_list.shape
assert q_AC_list.shape[1] == 4
q_AB_0 = Quaternion(q_AB_list[0, 0], q_AB_list[0, 1], q_AB_list[0, 2],
q_AB_list[0, 3])
q_AC_0 = Quaternion(q_AC_list[0, 0], q_AC_list[0, 1], q_AC_list[0, 2],
q_AC_list[0, 3])
orientation_errors_rad = np.zeros(q_AB_list.shape[0])
for idx in range(q_AB_list.shape[0]):
q_AB_xyzw = q_AB_list[idx, :]
q_AC_xyzw = q_AC_list[idx, :]
q_AB = Quaternion(q_AB_xyzw[0], q_AB_xyzw[1], q_AB_xyzw[2],
q_AB_xyzw[3])
q_AC = Quaternion(q_AC_xyzw[0], q_AC_xyzw[1], q_AC_xyzw[2],
q_AC_xyzw[3])
# Zero the (arbitary) initial poses.
q_AB = q_AB_0.inverse() * q_AB
q_AC = q_AC_0.inverse() * q_AC
q_BC = q_AB.inverse() * q_AC
angle_BC = q_BC.angle_axis()[3]
if angle_BC > np.pi:
angle_BC -= 2 * np.pi
assert q_AC_list.shape[1] == 4
assert angle_BC <= np.pi
assert angle_BC >= -np.pi
orientation_errors_rad[idx] = np.abs(angle_BC)
return ErrorStatistics(orientation_errors_rad)
def compute_orientation_mean_and_rmse(q_AB_list, q_AC_list):
assert q_AB_list.shape == q_AC_list.shape
assert (q_AC_list.shape[1] == 4)
orientation_errors_rad = np.zeros(q_AB_list.shape[0])
for idx in range(q_AB_list.shape[0]):
q_AB_xyzw = q_AB_list[idx, :]
q_AC_xyzw = q_AC_list[idx, :]
q_AB = Quaternion(q_AB_xyzw[0], q_AB_xyzw[1], q_AB_xyzw[2],
q_AB_xyzw[3])
q_AC = Quaternion(q_AC_xyzw[0], q_AC_xyzw[1], q_AC_xyzw[2],
q_AC_xyzw[3])
q_BC = q_AB.inverse() * q_AC
orientation_errors_rad[idx] = q_BC.angle_axis()[3]
return np.mean(orientation_errors_rad), RMSE(orientation_errors_rad)
def test_inverse(self):
q = Quaternion(1, 2, 3, 4)
q_identity = np.array([0, 0, 0, 1]).T
npt.assert_allclose((q * q.inverse()).q, q_identity)