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_from_aa(self):
# Axis should come out of rotation normalized
angle = 0.8
axis = [-3, 2, 1]
axis_norm = array_normalize(axis)
r = RotationD(angle, axis)
nose.tools.assert_equal(angle, r.angle())
numpy.testing.assert_equal(axis_norm, r.axis())
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_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_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_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_overrides(self):
rot_ = RotationD(0, [1, 0, 0])
intrins_ = ci(
focal_length=10.5,
principal_point=[3.14, 6.28],
aspect_ratio=1.2,
skew=3.1,
dist_coeffs=[4.5, 5.2, 6.8],
image_width=1080,
image_height=720,
)
cent = np.array([4, 3.14, 5.67])
cam_test = CameraPerspectiveImpl(rot_, cent, intrins_)
np.testing.assert_array_equal(cam_test.get_center(), cent)
nt.assert_equal(cam_test.rotation(), rot_)
np.testing.assert_array_equal(
cam_test.translation(),
-(cam_test.rot.inverse() * cam_test.center))
np.testing.assert_array_equal(
cam_test.center_covar().matrix(),
np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]))
nt.assert_equal(cam_test.intrinsics(), intrins_)
self.assertEqual(cam_test.image_height(), 720)
self.assertEqual(cam_test.image_width(), 1080)
np.testing.assert_array_equal(
cam_test.pose_matrix(), np.array([[1, 0, 0], [0, 1, 0], [0, 0,
1]]))
np.testing.assert_array_equal(
cam_test.as_matrix(), np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]))
pt = np.array([1, 2, 3])
np.testing.assert_array_equal(cam_test.project(pt), pt[:2])
np.testing.assert_array_equal(cam_test.depth(pt), pt[2:])
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_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_get_rotation(self):
s = SimilarityD()
numpy.testing.assert_array_almost_equal(s.rotation.matrix(),
RotationD().matrix())
s = SimilarityD(self.s, self.r, self.t)
numpy.testing.assert_array_almost_equal(s.rotation.matrix(),
self.r.matrix())
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 test_interpolated_rotations(self):
x = RotationD(0, [1, 0, 0])
a = math.pi / 2
y = RotationD(a, [0, 1, 0])
i_list = rotation.interpolated_rotations(x, y, 3)
nose.tools.assert_equal([i.type_name for i in i_list], ["d"] * 3)
i0_e_axis, i0_e_angle = [0, 1, 0], a * 0.25
i1_e_axis, i1_e_angle = [0, 1, 0], a * 0.50
i2_e_axis, i2_e_angle = [0, 1, 0], a * 0.75
numpy.testing.assert_almost_equal(i_list[0].axis(), i0_e_axis, 14)
numpy.testing.assert_almost_equal(i_list[0].angle(), i0_e_angle, 14)
numpy.testing.assert_almost_equal(i_list[1].axis(), i1_e_axis, 14)
numpy.testing.assert_almost_equal(i_list[1].angle(), i1_e_angle, 14)
numpy.testing.assert_almost_equal(i_list[2].axis(), i2_e_axis, 14)
numpy.testing.assert_almost_equal(i_list[2].angle(), i2_e_angle, 14)
def test_mul_vector(self):
vec = [1, 0, 0]
vec_expected = [0, 1, 0]
r_axis = [0, 0, 1]
r_angle = math.pi / 2.0
r = RotationD(r_angle, r_axis)
vec_rotated = r * vec
numpy.testing.assert_array_almost_equal(vec_expected, vec_rotated)
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 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_mul_sim(self):
s1 = SimilarityD(self.s, self.r, self.t)
s2 = SimilarityD(0.75, RotationD([-0.5, -0.5, 1.0]), [4, 6.5, 8])
sim_comp = (s1 * s2).matrix()
mat_comp = numpy.dot(s1.matrix(), s2.matrix())
print('sim12 comp:\n', sim_comp)
print('mat comp:\n', mat_comp)
print('sim - mat:\n', sim_comp - mat_comp)
nose.tools.assert_almost_equal(
numpy.linalg.norm(sim_comp - mat_comp, 2), 0., 12)
def twisted_pair_helper(self, em):
t_norm = self.translation / np.linalg.norm(self.translation)
r1 = em.rotation()
r2 = em.twisted_rotation()
t1 = em.translation()
t2 = -t1
rot_t_180 = RotationD(np.pi, t_norm)
np.testing.assert_array_almost_equal(
(rot_t_180 * r1).matrix(),
r2.matrix(),
6,
err_msg="Twisted pair rotation should be 180 degree rotation around t for type {}".format(
em.type_name
),
)
em1, em2, em3, em4 = (
EssentialMatrixD(r1, t1),
EssentialMatrixD(r1, t2),
EssentialMatrixD(r2, t1),
EssentialMatrixD(r2, t2),
)
m1, m2, m3, m4 = em1.matrix(), em2.matrix(), em3.matrix(), em4.matrix()
m = em.matrix()
nt.ok_(
self.check_arrays_similar(m, m1),
"Possible factorization 1 should match source for type: {}".format(
em.type_name
),
)
nt.ok_(
self.check_arrays_similar(m, m2),
"Possible factorization 2 should match source for type: {}".format(
em.type_name
),
)
nt.ok_(
self.check_arrays_similar(m, m3),
"Possible factorization 3 should match source for type: {}".format(
em.type_name
),
)
nt.ok_(
self.check_arrays_similar(m, m4),
"Possible factorization 4 should match source for type: {}".format(
em.type_name
),
)
def test_clone_clone_look_at(self):
rot_ = RotationD(0, [1, 0, 0])
intrins_ = ci(
focal_length=10.5,
principal_point=[3.14, 6.28],
aspect_ratio=1.2,
skew=3.1,
dist_coeffs=[4.5, 5.2, 6.8],
image_width=1080,
image_height=720,
)
cent = np.array([4, 3.14, 5.67])
cam_test = CameraPerspectiveImpl(rot_, cent, intrins_)
campp = cph.call_clone(cam_test)
self.assertIsInstance(campp, CameraPerspectiveImpl)
np.testing.assert_array_equal(campp.center, cent)
campp_look_at = cph.call_clone_look_at(cam_test, np.array([2, 3, 4]),
np.array([0, 0, 1]))
self.assertIsInstance(campp_look_at, CameraPerspectiveImpl)
nt.assert_equal(campp_look_at.rot, RotationD(0, [0, 1, 0]))
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_init(self):
rot_ = RotationD(0, [1, 0, 0])
intrins_ = ci(
focal_length=10.5,
principal_point=[3.14, 6.28],
aspect_ratio=1.2,
skew=3.1,
dist_coeffs=[4.5, 5.2, 6.8],
image_width=1080,
image_height=720,
)
cent = np.array([4, 3.14, 5.67])
CameraPerspectiveImpl(rot_, cent, intrins_)
def test_from_matrix(self):
# Create a non-identity matrix from a different constructor that we
# assume works
# Create new rotation with that matrix.
# New rotation to_matrix method should produce the same matrix
pre_r = RotationD([+2, -1, -3, +0])
mat = pre_r.matrix()
r = RotationD(mat)
numpy.testing.assert_allclose(mat, r.matrix(), 1e-15)
def noisy_cameras(cam_map, pos_stddev=1., rot_stddev=1.):
"""
Add positional and rotational gaussian noise to cameras
:type cam_map: CameraMap
:type pos_stddev: float
:type rot_stddev: float
:return: Camera map of new, noidy cameras'
"""
cmap = {}
for f, c in cam_map.as_dict().iteritems():
c2 = Camera(
c.center + random_point_3d(pos_stddev),
c.rotation * RotationD(random_point_3d(rot_stddev)),
c.intrinsics
)
cmap[f] = c2
return CameraMap(cmap)
def camera_seq(num_cams=20, k=None):
"""
Create a camera sequence (elliptical path)
:param num_cams: Number of cameras. Default is 20
:param k: Camera intrinsics to use for all created cameras. Default has
focal length = 1000 and principle point of (640, 480).
:return:
"""
if k is None:
k = CameraIntrinsics(1000, [640, 480])
d = {}
r = RotationD() # identity
for i in range(num_cams):
frac = float(i) / num_cams
x = 4 * math.cos(2*frac)
y = 3 * math.sin(2*frac)
d[i] = Camera([x, y, 2+frac], r, k).clone_look_at([0, 0, 0])
return CameraMap(d)
def init_cameras(num_cams=20, intrinsics=None):
"""
Initialize camera sequence with all cameras at the same location (0, 0, 1)
and looking at origin.
:param num_cams: Number of cameras to create, default 20.
:param intrinsics: Intrinsics to use for all cameras.
:return: Camera map of initialize cameras
"""
if intrinsics is None:
intrinsics = CameraIntrinsics(1000, (640, 480))
r = RotationD()
c = np.array([0, 0, 1])
d = {}
for i in range(num_cams):
cam = Camera(c, r, intrinsics).clone_look_at([0, 0, 0],
[0, 1, 0])
d[i] = cam
return CameraMap(d)
def setUp(self):
self.intrins = ci(
focal_length=10.5,
principal_point=[3.14, 6.28],
aspect_ratio=1.2,
skew=3.1,
dist_coeffs=[4.5, 5.2, 6.8],
image_width=1080,
image_height=720,
)
self.intrins_empty = ci()
self.rot = RotationD(0, [1, 0, 0])
self.vec = np.array([4, 3.14, 5.67])
m = np.eye(3, dtype=np.double)
m[0, 2] = 2.0
m_expected = m.copy()
m_expected[0, 2] = 1.0
m_expected[2, 0] = 1.0
self.covar3d = Covar3d(m)
def test_from_rodrigues(self):
rod_list_1 = [0, 0, 0]
r1 = RotationD(rod_list_1)
numpy.testing.assert_equal(r1.rodrigues(), rod_list_1)
# This one will get normalized by magnitude in rotation instance
# This vector's is less than 2*pi, so we should expect this vector to be
# returned as is.
rod2 = numpy.array([2, -1, 0.5])
nod2_normed = array_normalize(rod2)
print("r2 2-norm:", numpy.linalg.norm(rod2))
print("r2-normed:", nod2_normed)
r2 = RotationD(rod2)
numpy.testing.assert_array_almost_equal(
r2.rodrigues(), rod2, decimal=14, # 1e-14
)
def test_not_overidden(self):
# Test getters
scap_ = scap(center=self.vec,
rotation=self.rot,
intrinsics=self.intrins)
np.testing.assert_array_equal(scap_.get_center(), self.vec)
self.assertIsInstance(scap_.get_rotation(), RotationD)
nt.assert_equal(scap_.get_rotation(), self.rot)
self.assertIsInstance(scap_.get_intrinsics(), ci)
nt.assert_equal(scap_.get_intrinsics(), self.intrins)
# Test setters
new_center = np.array([0, 2, 0])
scap_.set_center(new_center)
np.testing.assert_array_equal(scap_.get_center(), new_center)
new_rot = RotationD(1, np.array([0, 2, 0]))
scap_.set_rotation(new_rot)
nt.assert_equal(scap_.get_rotation(), new_rot)
scap_.set_intrinsics(self.intrins_empty)
nt.assert_equal(scap_.get_intrinsics(), self.intrins_empty)
scap_.set_translation(np.array([1, 0, 3]))
np.testing.assert_array_equal(
scap_.get_center(),
-(scap_.get_rotation().inverse() * np.array([1, 0, 3])))
# Test covar setter/gettter
m_out = self.covar3d.matrix()
scap_.set_center_covar(self.covar3d)
np.testing.assert_array_equal(scap_.get_center_covar().matrix(), m_out)
# Test other (look_at)
scap_ = scap()
scap_.look_at(np.array([1, 0, 0]), np.array([0, 0, 1]))
rot_ = scap_.get_rotation()
np.testing.assert_array_almost_equal(
rot_.matrix(), np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]]))
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)