def normalise_quaternions(quat_dict):
norm_quat_dict = OrderedDict()
for k, v in quat_dict.items():
if v.shape[1] == 4:
norm_quats = []
for r in v:
q = np.array([r[1], r[2], r[3], r[0]]) # [x, y, z, w]
nq = Quat(normalize(q))
nq_v = nq._get_q()
norm_quats.append([nq_v[3], nq_v[0], nq_v[1],
nq_v[2]]) # [w, x, y, z]
norm_quat_dict[k] = np.array(norm_quats)
else:
norm_quat_dict[k] = v
return norm_quat_dict
def convert_quat_to_euler(quat_dict, k_list):
euler_dict = OrderedDict()
for k, v in quat_dict.items():
if v.shape[1] == 4 and k not in k_list:
euler_angles = []
for r in v:
q = np.array([r[1], r[2], r[3], r[0]]) # [x, y, z, w]
nq = Quat(normalize(q))
nq_v = nq._get_q()
w = nq_v[3]
x = nq_v[0]
y = nq_v[1]
z = nq_v[2]
# roll (x-axis rotation)
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(t0, t1)
# pitch (y-axis rotation)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch = math.asin(t2)
# yaw (z-axis rotation)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw = math.atan2(t3, t4)
euler_angles.append([roll, pitch, yaw])
euler_dict[k] = np.array(euler_angles)
else:
euler_dict[k] = v
return euler_dict
