def test_to_matrix(self):
# Default value should be identity
rot_d = RotationD()
numpy.testing.assert_array_equal(rot_d.matrix(), numpy.eye(3))
rot_f = RotationF()
numpy.testing.assert_array_equal(rot_f.matrix(), numpy.eye(3))
def test_to_ypr(self):
# ypr identity: (pi/2, 0, pi)
ident_ypr = (math.pi / 2, 0, -math.pi)
rot_d = RotationD()
rot_f = RotationF()
numpy.testing.assert_almost_equal(rot_d.yaw_pitch_roll(), ident_ypr, 15)
numpy.testing.assert_almost_equal(rot_f.yaw_pitch_roll(), ident_ypr)
def test_to_rodrigues(self):
# rodrigues identity: [0,0,0]
ident_rod = [0, 0, 0]
rot_d = RotationD()
rot_f = RotationF()
rod = rot_d.rodrigues()
numpy.testing.assert_equal(rod, ident_rod)
rod = rot_f.rodrigues()
numpy.testing.assert_equal(rod, ident_rod)
def test_interpolation(self):
x_d = RotationD(0, [1, 0, 0])
y_d = RotationD(math.pi / 2, [0, 1, 0])
r_d = RotationD(math.pi / 4, [0, 1, 0])
x_f = RotationF(0, [1, 0, 0])
y_f = RotationF(math.pi / 2, [0, 1, 0])
r_f = RotationF(math.pi / 4, [0, 1, 0])
z_d = rotation.interpolate_rotation(x_d, y_d, 0.5)
z_f = rotation.interpolate_rotation(x_f, y_f, 0.5)
nose.tools.assert_almost_equal((z_d.inverse() * r_d).angle(), 0, 14)
nose.tools.assert_almost_equal((z_f.inverse() * r_f).angle(), 0, 6)
def test_to_axis_angle(self):
# expected identity: [0,0,1] and 0
ident_axis = [0, 0, 1]
ident_angle = 0
rot_d = RotationD()
rot_f = RotationF()
numpy.testing.assert_equal(rot_d.axis(), ident_axis)
nose.tools.assert_equal(rot_d.angle(), ident_angle)
numpy.testing.assert_equal(rot_f.axis(), ident_axis)
nose.tools.assert_equal(rot_f.angle(), ident_angle)
def test_from_rotation(self):
r = RotationD()
r_cpy = RotationD(r)
nose.tools.ok_(r == r_cpy)
r = RotationD([1, 2, 3, 4])
r_cpy = RotationD(r)
nose.tools.ok_(r == r_cpy)
r = RotationF()
r_cpy = RotationF(r)
nose.tools.ok_(r == r_cpy)
r = RotationF([1, 2, 3, 4])
r_cpy = RotationF(r)
nose.tools.ok_(r == r_cpy)
def test_new_default(self):
# That these even construct
rot_d = RotationD()
nose.tools.assert_equal(rot_d.type_name, "d")
rot_f = RotationF()
nose.tools.assert_equal(rot_f.type_name, "f")
def test_enu_ned(self):
# ENU means x=east, y=north, z=up (default for general cv use)
# NED means x=north, y=east, z=down (convention for yaw-pitch-roll)
# Below are the ENU identity rotations in each coordinate system
ident_enu = (0, 0, 0)
ident_ned = (math.pi / 2, 0, math.pi)
# deals with equivalence of e.g. +pi and -pi with respect to orientation
def assert_angles_almost_equal(angles1, angles2, digits):
for angle1, angle2 in zip(angles1, angles2):
numpy.testing.assert_almost_equal(math.sin(angle1),
math.sin(angle2), digits)
numpy.testing.assert_almost_equal(math.cos(angle1),
math.cos(angle2), digits)
rot_d = rotation.enu_to_ned(RotationD())
rot_f = rotation.enu_to_ned(RotationF())
assert_angles_almost_equal(rot_d.yaw_pitch_roll(), ident_ned, 14)
assert_angles_almost_equal(rot_f.yaw_pitch_roll(), ident_ned, 6)
rot_d = rotation.ned_to_enu(rot_d)
rot_f = rotation.ned_to_enu(rot_f)
assert_angles_almost_equal(rot_d.yaw_pitch_roll(), ident_enu, 14)
assert_angles_almost_equal(rot_f.yaw_pitch_roll(), ident_enu, 6)
def test_mul(self):
# Normalize quaternaion vector.
expected_quat = array_normalize([+2.0, -1.0, -3.0, +0.0])
r_ident_d = RotationD()
r_ident_f = RotationF()
r_other_d = RotationD(expected_quat)
r_other_f = RotationF(expected_quat)
r_res_d = r_ident_d * r_other_d
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_f
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat, 1e-7)
def setUp(self):
# Matrices
self.rng = np.random.default_rng()
self.rand_float_mat = 10 * self.rng.random((3, 3), dtype="f") - 5
self.rand_double_mat = 10 * self.rng.random((3, 3), dtype="d") - 5
# Rotation and vector
self.rot_d = RotationD([1.0, 2.0, 3.0])
self.rot_f = RotationF([1.0, 2.0, 3.0])
self.translation = np.array([-1.0, 1.0, 4.0])
def test_not_eq(self):
# Identities should equal
r1 = RotationD()
r2 = RotationD()
nose.tools.assert_false(r1 != r2)
r3 = RotationD([1, 2, 3, 4])
r4 = RotationD([1, 2, 3, 4])
nose.tools.assert_false(r3 != r4)
nose.tools.ok_(r1 != r3)
r1 = RotationF()
r2 = RotationF()
nose.tools.assert_false(r1 != r2)
r3 = RotationF([1, 2, 3, 4])
r4 = RotationF([1, 2, 3, 4])
nose.tools.assert_false(r3 != r4)
nose.tools.ok_(r1 != r3)
def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = RotationD([+2, -1, -3, +0])
r_inv = r.inverse()
e_inv = array_normalize([-1 / 7.0, +1 / 14.0, +3 / 14.0, 0])
numpy.testing.assert_allclose(r_inv.quaternion(), e_inv, 1e-15)
r = RotationF([+2, -1, -3, +0])
r_inv = r.inverse()
numpy.testing.assert_allclose(r_inv.quaternion(), e_inv, 1e-7)
def test_eq(self):
# Identities should equal
r1 = RotationD()
r2 = RotationD()
nose.tools.assert_equal(r1, r2)
r3 = RotationD([1, 2, 3, 4])
r4 = RotationD([1, 2, 3, 4])
nose.tools.assert_equal(r3, r4)
nose.tools.assert_false(r1 == r3)
r1 = RotationF()
r2 = RotationF()
nose.tools.assert_equal(r1, r2)
r3 = RotationF([1, 2, 3, 4])
r4 = RotationF([1, 2, 3, 4])
nose.tools.assert_equal(r3, r4)
nose.tools.assert_false(r1 == r3)
r1 = RotationD([1, 2, 3, 4])
r2 = RotationD([-1, -2, -3, -4])
assert r1.angle_from(r2) < 1e-12
def test_new_default(self):
self.check_members_equal(SimilarityD(), 'd', 1,
RotationD().matrix(), [0, 0, 0], 15)
self.check_members_equal(SimilarityF(), 'f', 1,
RotationF().matrix(), [0, 0, 0], 6)
def setUpClass(cls):
cls.s = 2.4
cls.r = RotationD([0.1, -1.5, 2.0])
cls.r_f = RotationF([0.1, -1.5, 2.0])
cls.t = [1, -2, 5]
def test_to_quaternion(self):
rot_d = RotationD()
numpy.testing.assert_array_equal(rot_d.quaternion(), [0, 0, 0, 1])
rot_f = RotationF()
numpy.testing.assert_array_equal(rot_f.quaternion(), [0, 0, 0, 1])
def test_from_rotation_other_type(self):
r = RotationD()
r_cpy = RotationF(r)
numpy.testing.assert_array_almost_equal(r.quaternion(), r_cpy.quaternion(), 6)
r = RotationD([1, 2, 3, 4])
r_cpy = RotationF(r)
numpy.testing.assert_array_almost_equal(r.quaternion(), r_cpy.quaternion(), 6)
r = RotationF()
r_cpy = RotationD(r)
numpy.testing.assert_array_almost_equal(r.quaternion(), r_cpy.quaternion(), 6)
r = RotationF([1, 2, 3, 4])
r_cpy = RotationD(r)
numpy.testing.assert_array_almost_equal(r.quaternion(), r_cpy.quaternion(), 6)