def test_unique_representation_quat(self): a = 1.2345 # These quaternions represent equivalent roatations l = 1/numpy.sqrt(3) q_same = [rotation.unique_representation_quat(rotation.quat(a, l, l, l)), rotation.unique_representation_quat(rotation.quat(-a, -l, -l, -l)), rotation.unique_representation_quat(rotation.quat(a + 2*numpy.pi, l, l, l)), rotation.unique_representation_quat(rotation.quat(a - 2*numpy.pi, l, l, l))] q_expected = q_same[0] for q in q_same[1:]: for q_i, q_expected_i in zip(q, q_expected): self.assertAlmostEqual(q_i, q_expected_i)
def test_unique_representation_quat(self): a = 1.2345 # These quaternions represent equivalent roatations l = 1 / numpy.sqrt(3) q_same = [ rotation.unique_representation_quat(rotation.quat(a, l, l, l)), rotation.unique_representation_quat(rotation.quat(-a, -l, -l, -l)), rotation.unique_representation_quat( rotation.quat(a + 2 * numpy.pi, l, l, l)), rotation.unique_representation_quat( rotation.quat(a - 2 * numpy.pi, l, l, l)) ] q_expected = q_same[0] for q in q_same[1:]: for q_i, q_expected_i in zip(q, q_expected): self.assertAlmostEqual(q_i, q_expected_i)
def test_rotation_conversions_betw_formalisms(self, N=1000): """ Create random euler angles with different axes formalisms and test conversion in a circular fashion. Euler angles => rotation matrix => quaternion => unique representation of quaternion (q_a) => Euler angles => rotation materix => quaterinion => unique representation of quaternion (q_b). Finally compare the two quaternions q_a and q_b. Repeat N times. """ rotation_matrices = { 'xyz': (rotation.R_x, rotation.R_y, rotation.R_z), 'xzy': (rotation.R_x, rotation.R_z, rotation.R_y), 'yxz': (rotation.R_y, rotation.R_x, rotation.R_z), 'yzx': (rotation.R_y, rotation.R_z, rotation.R_x), 'zxy': (rotation.R_z, rotation.R_x, rotation.R_y), 'zyx': (rotation.R_z, rotation.R_y, rotation.R_x), 'yxy': (rotation.R_y, rotation.R_x, rotation.R_y), 'zxz': (rotation.R_z, rotation.R_x, rotation.R_z), 'xyx': (rotation.R_x, rotation.R_y, rotation.R_x), 'zyz': (rotation.R_z, rotation.R_y, rotation.R_z), 'xzx': (rotation.R_x, rotation.R_z, rotation.R_x), 'yzy': (rotation.R_y, rotation.R_z, rotation.R_y), } succ = 0 NN = len(rotation_matrices) * N err = numpy.ones(NN) k = 0 for ax, (R1, R2, R3) in rotation_matrices.items(): for i in range(N): euler_a = numpy.random.random(3) * numpy.pi * 2 euler_a[1] /= 2. # Euler angles => rotaion matrix R_a = R1(euler_a[0]).dot(R2(euler_a[1]).dot(R3(euler_a[2]))) # Rotation matrix => unique quaternion q_a = rotation.quat_from_rotmx(R_a) q_a = rotation.unique_representation_quat(q_a) # Unique quaternion => Euler angles euler_b = rotation.euler_from_quat(q_a, ax) # Euler angles => quaternion R_b = R1(euler_b[0]).dot(R2(euler_b[1]).dot(R3(euler_b[2]))) # Rotation matrix => unique quaternion q_b = rotation.quat_from_rotmx(R_b) q_b = rotation.unique_representation_quat(q_b) # Calculate discrepancy for q_a_i, q_b_i in zip(q_a, q_b): self.assertAlmostEqual(q_a_i, q_b_i)
def test_rotation_conversions_betw_formalisms(self, N=1000): """ Create random euler angles with different axes formalisms and test conversion in a circular fashion. Euler angles => rotation matrix => quaternion => unique representation of quaternion (q_a) => Euler angles => rotation materix => quaterinion => unique representation of quaternion (q_b). Finally compare the two quaternions q_a and q_b. Repeat N times. """ rotation_matrices = { 'xyz': (rotation.R_x, rotation.R_y, rotation.R_z), 'xzy': (rotation.R_x, rotation.R_z, rotation.R_y), 'yxz': (rotation.R_y, rotation.R_x, rotation.R_z), 'yzx': (rotation.R_y, rotation.R_z, rotation.R_x), 'zxy': (rotation.R_z, rotation.R_x, rotation.R_y), 'zyx': (rotation.R_z, rotation.R_y, rotation.R_x), 'yxy': (rotation.R_y, rotation.R_x, rotation.R_y), 'zxz': (rotation.R_z, rotation.R_x, rotation.R_z), 'xyx': (rotation.R_x, rotation.R_y, rotation.R_x), 'zyz': (rotation.R_z, rotation.R_y, rotation.R_z), 'xzx': (rotation.R_x, rotation.R_z, rotation.R_x), 'yzy': (rotation.R_y, rotation.R_z, rotation.R_y), } succ = 0 NN = len(rotation_matrices)*N err = numpy.ones(NN) k = 0 for ax,(R1,R2,R3) in rotation_matrices.items(): for i in range(N): euler_a = numpy.random.random(3) * numpy.pi * 2 euler_a[1] /= 2. # Euler angles => rotaion matrix R_a = R1(euler_a[0]).dot(R2(euler_a[1]).dot(R3(euler_a[2]))) # Rotation matrix => unique quaternion q_a = rotation.quat_from_rotmx(R_a) q_a = rotation.unique_representation_quat(q_a) # Unique quaternion => Euler angles euler_b = rotation.euler_from_quat(q_a, ax) # Euler angles => quaternion R_b = R1(euler_b[0]).dot(R2(euler_b[1]).dot(R3(euler_b[2]))) # Rotation matrix => unique quaternion q_b = rotation.quat_from_rotmx(R_b) q_b = rotation.unique_representation_quat(q_b) # Calculate discrepancy for q_a_i, q_b_i in zip(q_a, q_b): self.assertAlmostEqual(q_a_i, q_b_i)