def test_quaternionFromMatrix_4D(self):
"""Simple rotations with 4D input"""
# This is identical to simple tests, but with a different shape
# Identity, and rotations by 90 degrees around X, Y, and Z axis
inputs = np.array([
[[[1, 0, 0], [0, 1, 0], [0, 0, 1]],
[[1, 0, 0], [0, 0, -1], [0, 1, 0]]],
[[[0, 0, 1], [0, 1, 0], [-1, 0, 0]],
[[0, -1, 0], [1, 0, 0], [0, 0, 1]]],
])
cos45 = 0.5 ** 0.5 # 1/sqrt(2), or cos/sin of 45 degrees
expected = np.array([
[[0, 0, 0, 1],
[cos45, 0, 0, cos45]],
[[0, cos45, 0, cos45],
[0, 0, cos45, cos45]],
])
actual = spc.quaternionFromMatrix(inputs)
np.testing.assert_array_almost_equal(actual, expected)
# Put scalar on other side
expected = np.array([
[[1, 0, 0, 0],
[cos45, cos45, 0, 0]],
[[cos45, 0, cos45, 0],
[cos45, 0, 0, cos45]],
])
actual = spc.quaternionFromMatrix(inputs, scalarPos='first')
np.testing.assert_array_almost_equal(actual, expected)
def test_quaternionFromMatrix_errors(self):
"""Test bad input"""
matrix = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
with self.assertRaises(NotImplementedError) as cm:
spc.quaternionFromMatrix(matrix, 'FOO')
self.assertEqual(
'quaternionFromMatrix: scalarPos must be set to "First" or "Last"',
str(cm.exception))
with self.assertRaises(ValueError) as cm:
spc.quaternionFromMatrix([[1, 2, 3]])
self.assertEqual(
'Input does not appear to be 3D rotation matrix, wrong size.',
str(cm.exception))
with self.assertRaises(ValueError) as cm:
spc.quaternionFromMatrix(
[[1, 1, 1], [2, 2, 2], [3, 3, 3]])
self.assertEqual(
'Input rotation matrix not orthogonal.',
str(cm.exception))
with self.assertRaises(ValueError) as cm:
spc.quaternionFromMatrix([
[[1, 0, 0], [0, 1, 0], [0, 0, 1]],
[[1, 1, 1], [2, 2, 2], [3, 3, 3]]
])
self.assertEqual(
'Input rotation matrix at (1,) not orthogonal.',
str(cm.exception))
with self.assertRaises(ValueError) as cm:
spc.quaternionFromMatrix(
[[1, 0, 0], [0, -1, 0], [0, 0, 1]])
self.assertEqual(
'Input rotation matrix at () not proper.',
str(cm.exception))
def test_quaternionFromMatrix_simple(self):
"""Test several simple rotations"""
# Identity, and rotations by 90 degrees around X, Y, and Z axis
inputs = np.array([
[[1, 0, 0], [0, 1, 0], [0, 0, 1]],
[[1, 0, 0], [0, 0, -1], [0, 1, 0]],
[[0, 0, 1], [0, 1, 0], [-1, 0, 0]],
[[0, -1, 0], [1, 0, 0], [0, 0, 1]],
])
cos45 = 0.5 ** 0.5 # 1/sqrt(2), or cos/sin of 45 degrees
expected = np.array([
[0, 0, 0, 1],
[cos45, 0, 0, cos45],
[0, cos45, 0, cos45],
[0, 0, cos45, cos45],
])
# Test single rotation at a time
for i in range(expected.shape[0]):
np.testing.assert_array_almost_equal(
spc.quaternionFromMatrix(inputs[i, ...]),
expected[i, ...])
# Whole array at once
actual = spc.quaternionFromMatrix(inputs)
np.testing.assert_array_almost_equal(actual, expected)
# Put scalar on other side
expected = np.array([
[1, 0, 0, 0],
[cos45, cos45, 0, 0],
[cos45, 0, cos45, 0],
[cos45, 0, 0, cos45],
])
actual = spc.quaternionFromMatrix(inputs, scalarPos='first')
np.testing.assert_array_almost_equal(actual, expected)
def test_quaternionFromMatrix_nasty(self):
"""Pick an arbitrary rotation and verify it works"""
# https://csm.mech.utah.edu/content/wp-content/uploads/2011/08/orthList.pdf
# Axis of rotation
u = [12. / 41, -24. / 41, 31. / 41]
# Rotation angle
theta = np.radians(58)
ux, uy, uz = u
c = np.cos(theta)
s = np.sin(theta)
# Construct rotation matrix from axis and angle
# This might be doable more nicely in matrix notation...
matrix = np.array([
[c + ux ** 2 * (1 - c),
ux * uy * (1 - c) - uz * s,
ux * uz * (1 - c) + uy * s],
[uy * ux * (1 - c) + uz * s,
c + uy ** 2 * (1 - c),
uy * uz * (1 - c) - ux * s],
[uz * ux * (1 - c) - uy * s,
uz * uy * (1 - c) + ux * s,
c + uz ** 2 * (1 - c)]
])
Qout = spc.quaternionFromMatrix(matrix)
# Sample inputs to rotate
invect = np.array([[5, 3, 2], [1, 0, 0], [.2, 5, 20],
[0, 2, 2]])
# Transform the row vectors into column vectors so the
# numpy multiplication gives the right result (then
# transform back to row vectors for comparison.)
expected = np.dot(matrix, invect.transpose()).transpose()
actual = spc.quaternionRotateVector(
np.tile(Qout, (4, 1)), invect)
np.testing.assert_array_almost_equal(
actual, expected)
def testQuaternionToMatrixRT(self):
"""Round-trip test quaternion to matrix and back"""
# Numbers pulled out of air
Qin = spc.quaternionNormalize(np.array([0.25, 0.5, 0.71, 0.25]))
matrix = spc.quaternionToMatrix(Qin)
Qrt = spc.quaternionFromMatrix(matrix)
if np.sign(Qrt[-1]) != np.sign(Qin[-1]):
Qrt *= -1 #Handle the sign ambiguity
np.testing.assert_array_almost_equal(Qrt, Qin)
def test_quaternionFromMatrix_perturbed(self):
"""Add error to a rotation matrix"""
# Rotation by 90 degrees around X axis
matrix = np.array([[1., 0, 0], [0, 0, -1], [0, 1, 0]], )
cos45 = 0.5**0.5 # 1/sqrt(2), or cos/sin of 45 degrees
# Equivalent quaternion
expected = np.array([cos45, 0, 0, cos45], )
# Add error, make sure still comes up with something reasonable
np.random.seed(0x0d15ea5e)
err = np.random.rand(3, 3) / 50 - 0.01 #-0.01 to 0.01
matrix += err
actual = spc.quaternionFromMatrix(matrix)
np.testing.assert_array_almost_equal(actual, expected, decimal=3)
def test_quaternionFromMatrix_rt(self):
"""Round-trip arbitrary rotation matrix to quaternion and back"""
# Same matrix as test_quaternionFromMatrix_nasty
u = [12. / 41, -24. / 41, 31. / 41]
theta = np.radians(58)
ux, uy, uz = u
c = np.cos(theta)
s = np.sin(theta)
matrix = np.array([
[c + ux ** 2 * (1 - c),
ux * uy * (1 - c) - uz * s,
ux * uz * (1 - c) + uy * s],
[uy * ux * (1 - c) + uz * s,
c + uy ** 2 * (1 - c),
uy * uz * (1 - c) - ux * s],
[uz * ux * (1 - c) - uy * s,
uz * uy * (1 - c) + ux * s,
c + uz ** 2 * (1 - c)]
])
Qout = spc.quaternionFromMatrix(matrix)
matrix_rt = spc.quaternionToMatrix(Qout)
np.testing.assert_array_almost_equal(
matrix_rt, matrix)