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