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)