def test_quat2mat():
# also tested in roundtrip case below
M = nq.quat2mat([1, 0, 0, 0])
yield assert_array_almost_equal, M, np.eye(3)
M = nq.quat2mat([3, 0, 0, 0])
yield assert_array_almost_equal, M, np.eye(3)
M = nq.quat2mat([0, 1, 0, 0])
yield assert_array_almost_equal, M, np.diag([1, -1, -1])
M = nq.quat2mat([0, 2, 0, 0])
yield assert_array_almost_equal, M, np.diag([1, -1, -1])
M = nq.quat2mat([0, 0, 0, 0])
yield assert_array_almost_equal, M, np.eye(3)
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 sensbiotk.transforms3d.quaternions as nq
return mat2euler(nq.quat2mat(q))
def test_quaternion_reconstruction():
# Test reconstruction of arbitrary unit quaternions
for q in unit_quats:
M = nq.quat2mat(q)
qt = nq.mat2quat(M)
# Accept positive or negative match
posm = np.allclose(q, qt)
negm = np.allclose(q, -qt)
yield assert_true, posm or negm
def test_inverse():
# Takes sequence
iq = nq.inverse((1, 0, 0, 0))
# Returns float type
yield assert_true, iq.dtype.kind == 'f'
for M, q in eg_pairs:
iq = nq.inverse(q)
iqM = nq.quat2mat(iq)
iM = np.linalg.inv(M)
yield assert_true, np.allclose(iM, iqM)
def test_eye():
qi = nq.eye()
yield assert_true, np.all([1, 0, 0, 0] == qi)
yield assert_true, np.allclose(nq.quat2mat(qi), np.eye(3))
def test_inverse():
for M, q in eg_pairs:
iq = nq.inverse(q)
iqM = nq.quat2mat(iq)
iM = np.linalg.inv(M)
yield assert_true, np.allclose(iM, iqM)
def test_mult():
# Test that quaternion * same as matrix *
for M1, q1 in eg_pairs[0::4]:
for M2, q2 in eg_pairs[1::4]:
q21 = nq.mult(q2, q1)
yield assert_array_almost_equal, np.dot(M2, M1), nq.quat2mat(q21)