def test_quat2rot2quat():
"""Test quaternion2rot3d and rotmat_to_quaternion consistency."""
n = 1000
orientations = generate.get_random_quat(n)
for i in range(n):
orientation = orientations[i]
rot = convert.quaternion2rot3d(orientation)
quat = convert.rotmat_to_quaternion(rot)
assert np.allclose(orientation, quat) \
or np.allclose(orientation, -quat)
def test_quaternion_to_angle_axis_to_quaternion():
"""Test quaternion_to_angle_axis and reverse for consistency."""
n = 1000
orientations = generate.get_random_quat(n)
for i in range(n):
orientation = orientations[i]
theta, axis = convert.quaternion_to_angle_axis(orientation)
quat = convert.angle_axis_to_quaternion(axis, theta)
assert np.allclose(orientation, quat) \
or np.allclose(orientation, -quat)
def test_quaternion2rot3d_invariant():
"""Test the invariance of quaternion2rot3d.
Test the invariance property of the rotation axis on randomly
selected axes.
"""
n = 1000
orientations = generate.get_random_quat(n)
for i in range(n):
orientation = orientations[i]
rot = convert.quaternion2rot3d(orientation)
rotated = np.dot(rot, orientation[1:])
assert np.allclose(rotated, orientation[1:])
def test_angle_axis_to_rot3d_invariant():
"""Test the invariance of angle_axis_to_rot3d.
Test the invariance property of the rotation axis on randomly
selected axes.
"""
n = 1000
orientations = generate.get_random_quat(n)
thetas = np.random.rand(n) * 2 * np.pi
for i in range(n):
orientation = orientations[i, 1:]
theta = thetas[i]
rot = convert.angle_axis_to_rot3d(orientation, theta)
rotated = np.dot(rot, orientation)
assert np.allclose(rotated, orientation)
