def quat2euler(q):
''' Return Euler angles corresponding to quaternion `q`
Parameters
----------
q : 4 element sequence
w, x, y, z of quaternion
Returns
-------
z : scalar
Rotation angle in radians around z-axis (performed first)
y : scalar
Rotation angle in radians around y-axis
x : scalar
Rotation angle in radians around x-axis (performed last)
Notes
-----
It's possible to reduce the amount of calculation a little, by
combining parts of the ``quat2mat`` and ``mat2euler`` functions, but
the reduction in computation is small, and the code repetition is
large.
'''
# delayed import to avoid cyclic dependencies
import nibabel.quaternions as nq
return mat2euler(nq.quat2mat(q))
def quat2euler(q):
''' Return Euler angles corresponding to quaternion `q`
Parameters
----------
q : 4 element sequence
w, x, y, z of quaternion
Returns
-------
z : scalar
Rotation angle in radians around z-axis (performed first)
y : scalar
Rotation angle in radians around y-axis
x : scalar
Rotation angle in radians around x-axis (performed last)
Notes
-----
It's possible to reduce the amount of calculation a little, by
combining parts of the ``quat2mat`` and ``mat2euler`` functions, but
the reduction in computation is small, and the code repetition is
large.
'''
# delayed import to avoid cyclic dependencies
import nibabel.quaternions as nq
return mat2euler(nq.quat2mat(q))
def try_rotations_quat():
head1, pitch1, roll1 = 140, 40, 0
head2, pitch2, roll2 = 75, 60, 0
q1 = ru.euler2quat(head1, pitch1, roll1, isRadian=False)
q2 = ru.euler2quat(head2, pitch2, roll2, isRadian=False)
mat1 = nq.quat2mat(q1)
mat2 = nq.quat2mat(q2)
q3 = q2
q3[0] = -q3[0]
mat = np.dot(mat2.transpose(), mat1)
dMat = nq.quat2mat(nq.mult(q3, q1))
diff = linalg.norm(linalg.logm(np.dot(np.transpose(mat), dMat)), ord='fro')
print diff
print(mat - dMat)
h, p, r = ru.mat2euler(mat)
print(h, p, r)
return h, p, r
def pose2T(pose):
import nibabel.quaternions as nq
R = nq.quat2mat(pose[3:])
T = np.array([[R[0,0], R[0,1], R[0,2], pose[0]],
[R[1,0], R[1,1], R[1,2], pose[1]],
[R[2,0], R[2,1], R[2,2], pose[2]],
[0,0,0,1]])
# T = np.vstack((T, [0, 0, 0, 1]))
return T
def quat2euler(quaternion_angles, show_angles=False, Rinit=np.eye(3)):
euler_angles = np.zeros((quaternion_angles.shape[0], 3))
for i, angle in enumerate(quaternion_angles):
euler_angles[i, :] = mat2euler(np.dot(Rinit, qu.quat2mat(angle)))
if show_angles:
colors = ['r*-', 'g*-', 'b*-']
plt.figure()
for i, c in enumerate(colors):
plt.plot(euler_angles[:, i], c)
plt.legend(['yaw', 'pitch', 'roll'])
plt.title('The Euler angles from the csv file')
sns.despine(trim=True)
plt.show()
return euler_angles
def pose_dict_to_arr(pose):
pm, qm = pose['p'], pose['q']
p = np.array([pm['x'], pm['y'], pm['z']])
q = np.array([qm['qw'], qm['qx'], qm['qy'], qm['qz']])
rot = quat.quat2mat(q)
return p, q, rot
