def test_to_matrix(self):
# Default value should be identity
rot_d = Rotation('d')
numpy.testing.assert_array_equal(rot_d.matrix(), numpy.eye(3))
rot_f = Rotation('f')
numpy.testing.assert_array_equal(rot_f.matrix(), numpy.eye(3))
def test_new_default(self):
# That these even construct
rot_d = Rotation('d')
nose.tools.assert_equal(rot_d._ctype, 'd')
rot_f = Rotation('f')
nose.tools.assert_equal(rot_f._ctype, 'f')
def test_to_matrix(self):
# Default value should be identity
rot_d = Rotation(ctypes.c_double)
numpy.testing.assert_array_equal(rot_d.matrix(), numpy.eye(3))
rot_f = Rotation(ctypes.c_float)
numpy.testing.assert_array_equal(rot_f.matrix(), numpy.eye(3))
def test_to_quaternion(self):
rot_d = Rotation(ctypes.c_double)
numpy.testing.assert_array_equal(rot_d.quaternion(),
[[0], [0], [0], [1]])
rot_f = Rotation(ctypes.c_float)
numpy.testing.assert_array_equal(rot_f.quaternion(),
[[0], [0], [0], [1]])
def test_new_default(self):
# That these even construct
rot_d = Rotation(ctypes.c_double)
nose.tools.assert_equal(rot_d._ctype, ctypes.c_double)
nose.tools.assert_equal(rot_d._spec, 'd')
rot_f = Rotation(ctypes.c_float)
nose.tools.assert_equal(rot_f._ctype, ctypes.c_float)
nose.tools.assert_equal(rot_f._spec, 'f')
def test_to_ypr(self):
# ypr identity: (pi/2, 0, pi)
ident_ypr = (math.pi / 2, 0, -math.pi)
ident_ypr_float = [ctypes.c_float(v).value for v in ident_ypr]
rot_d = Rotation(ctypes.c_double)
rot_f = Rotation(ctypes.c_float)
numpy.testing.assert_equal(rot_d.yaw_pitch_roll(), ident_ypr)
numpy.testing.assert_equal(rot_f.yaw_pitch_roll(), ident_ypr_float)
def test_to_ypr(self):
# ypr identity: (pi/2, 0, pi)
ident_ypr = (math.pi / 2, 0, -math.pi)
ident_ypr_float = [float(v) for v in ident_ypr]
rot_d = Rotation('d')
rot_f = Rotation('f')
numpy.testing.assert_almost_equal(rot_d.yaw_pitch_roll(), ident_ypr)
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 = Rotation(ctypes.c_double)
rot_f = Rotation(ctypes.c_float)
rod = rot_d.rodrigues()
numpy.testing.assert_equal(rod, ident_rod)
rod = rot_f.rodrigues()
numpy.testing.assert_equal(rod, ident_rod)
def test_to_rodrigues(self):
# rodrigues identity: [0,0,0]
ident_rod = [0, 0, 0]
rot_d = Rotation('d')
rot_f = Rotation('f')
rod = rot_d.rodrigues()
numpy.testing.assert_equal(rod, ident_rod)
rod = rot_f.rodrigues()
numpy.testing.assert_equal(rod, ident_rod)
def test_compose(self):
expected_quat = [[+2], [-1], [-3], [+0]]
r_ident_d = Rotation(ctypes.c_double)
r_ident_f = Rotation(ctypes.c_float)
r_other_d = Rotation.from_quaternion(expected_quat, ctypes.c_double)
r_other_f = Rotation.from_quaternion(expected_quat, ctypes.c_float)
r_res_d = r_ident_d.compose(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.compose(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_equal(r_res_f.quaternion(), expected_quat)
# Should also work with multiply operator
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_equal(r_res_f.quaternion(), expected_quat)
# Rotation of non-congruent types should be converted automatically
r_res_d = r_ident_d.compose(r_other_f)
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_equal(r_res_d.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_d)
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
r_res_d = r_ident_d * r_other_f
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_equal(r_res_d.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_d
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
def test_to_axis_angle(self):
# expected identity: [0,0,1] and 0
ident_axis = [0, 0, 1]
ident_angle = 0
rot_d = Rotation('d')
rot_f = Rotation('f')
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_to_axis_angle(self):
# expected identity: [0,0,1] and 0
ident_axis = [[0], [0], [1]]
ident_angle = 0
rot_d = Rotation(ctypes.c_double)
rot_f = Rotation(ctypes.c_float)
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 _new(self, center, rotation, intrinsics):
cam_new = self.VITAL_LIB['vital_camera_new']
cam_new.argtypes = [
EigenArray.c_ptr_type(3, 1, ctypes.c_double),
Rotation.c_ptr_type(ctypes.c_double),
CameraIntrinsics.c_ptr_type(),
VitalErrorHandle.c_ptr_type()
]
cam_new.restype = self.c_ptr_type()
# Fill in parameter gaps
if center is None:
center = EigenArray(3)
center[:] = 0
else:
center = EigenArray.from_iterable(center)
if rotation is None:
rotation = Rotation()
if intrinsics is None:
intrinsics = CameraIntrinsics()
with VitalErrorHandle() as eh:
return cam_new(center, rotation, intrinsics, eh)
def test_new(self):
# just seeing that basic construction doesn't blow up
cam = Camera()
c = EigenArray(3)
r = Rotation()
ci = CameraIntrinsics()
cam = Camera(c, r, ci)
def test_get_rotation(self):
s = Similarity()
numpy.testing.assert_array_almost_equal(s.rotation.matrix(),
Rotation().matrix())
s = Similarity(self.s, self.r, self.t)
numpy.testing.assert_array_almost_equal(s.rotation.matrix(),
self.r.matrix())
def rotation(self):
"""
Get the rotation of this similarity transformation
:rtype: Rotation
"""
rot_ptr = self._call_cfunc(
'vital_similarity_%s_rotation' % self._tchar, [self.C_TYPE_PTR],
[self], Rotation.c_ptr_type(self._ctype))
return Rotation(self._ctype, rot_ptr)
def rotation(self):
"""
:return: a copy of this camera's rotation
:rtype: Rotation
"""
cam_rot = self.VITAL_LIB['vital_camera_rotation']
cam_rot.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_rot.restype = Rotation.c_ptr_type()
with VitalErrorHandle() as eh:
c_ptr = cam_rot(self, eh)
return Rotation(from_cptr=c_ptr)
def test_clone(self):
# clone should have the same values as original but be a different
# memory address.
R = Rotation()
K = CameraIntrinsics(1000, (640, 480))
c1 = Camera((0, 3, 5), R, K)
c2 = c1.clone()
# Different mem address
nose.tools.assert_not_equal(ctypes.addressof(c1.c_pointer.contents),
ctypes.addressof(c2.c_pointer.contents))
numpy.testing.assert_equal(c1.center, c2.center)
nose.tools.assert_equal(c1.rotation, c2.rotation)
nose.tools.assert_equal(c1.intrinsics, c2.intrinsics)
def test_clone_look_at(self):
pp = EigenArray.from_iterable([300, 400])
k = CameraIntrinsics(1000, pp)
focus = EigenArray.from_iterable([0, 1, -2])
base = Camera([3, -4, 7], Rotation(), k)
cam = base.clone_look_at(focus)
nose.tools.assert_not_equal(base, cam)
ifocus = cam.project(focus)
nose.tools.assert_almost_equal(numpy.linalg.norm(ifocus - pp, 2), 0.,
12)
ifocus_up = cam.project(focus + EigenArray.from_iterable([0, 0, 2]))
tmp = ifocus_up - pp
nose.tools.assert_almost_equal(tmp[0], 0., 12)
nose.tools.assert_true(tmp[1] < 0.)
def test_clone_look_at(self):
pp = EigenArray.from_array([[300], [400]])
k = CameraIntrinsics(1000, [300, 400])
focus = EigenArray.from_array([[0], [1], [-2]])
center = EigenArray.from_array([[3], [-4], [7]])
base = Camera(center, Rotation(), k)
cam = base.clone_look_at(numpy.array([0, 1, 2]))
nose.tools.assert_not_equal(base, cam)
ifocus = cam.project([0, 1, 2])
nose.tools.assert_almost_equal(
numpy.linalg.norm(ifocus - pp.get_matrix().T, 2), 0., 12)
ifocus_up = cam.project([0, 1, 4])
tmp = (ifocus_up - pp.get_matrix().T)[0]
nose.tools.assert_almost_equal(tmp[0], 0., 12)
nose.tools.assert_true(tmp[1] < 0.)
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 = Rotation() # identity
for i in xrange(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 = Rotation()
c = EigenArray.from_iterable((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 test_eq(self):
# Identities should equal
r1 = Rotation(ctypes.c_double)
r2 = Rotation(ctypes.c_double)
nose.tools.assert_equal(r1, r2)
r1 = Rotation(ctypes.c_float)
r2 = Rotation(ctypes.c_float)
nose.tools.assert_equal(r1, r2)
r1 = Rotation(ctypes.c_double)
r2 = Rotation(ctypes.c_float)
# r2 should get converted into a double instance for checking
nose.tools.assert_equal(r1, r2)
def _new(self, s, r, t):
"""
:type s: float
:type r: Rotation | None
:type t: collections.Iterable[float] | None
"""
# noinspection PyProtectedMember
if r is None:
r = Rotation(self._ctype)
elif r._ctype != self._ctype:
# Create new Rotation sharing our type
r = Rotation.from_quaternion(r.quaternion(), self._ctype)
if t is None:
t = EigenArray.from_iterable((0, 0, 0), self._ctype)
else:
t = EigenArray.from_iterable(t, self._ctype, (3, 1))
return self._call_cfunc('vital_similarity_%s_new' % self._tchar, [
self._ctype,
Rotation.c_ptr_type(self._ctype),
EigenArray.c_ptr_type(3, 1, self._ctype)
], [s, r, t], self.C_TYPE_PTR)
def test_eq(self):
# Identities should equal
r1 = Rotation('d')
r2 = Rotation('d')
nose.tools.assert_equal(r1, r2)
r1 = Rotation('f')
r2 = Rotation('f')
nose.tools.assert_equal(r1, r2)
r1 = Rotation('d')
r2 = Rotation('f')
# r2 should get converted into a double instance for checking
nose.tools.assert_equal(r1, r2)
r1 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d')
r2 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d')
nose.tools.assert_equal(r1, r2)
r1 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d')
r2 = Rotation.from_quaternion([-1, -2, -3, -4], ctype='d')
assert r1.angle_from(r2) < 1e-12
def test_get_rotation(self):
s = Similarity()
nose.tools.assert_equal(s.rotation, Rotation())
s = Similarity(self.s, self.r, self.t)
nose.tools.assert_equal(s.rotation, self.r)
def test_new_default(self):
s = Similarity()
nose.tools.assert_equal(s.scale, 1)
nose.tools.assert_equal(s.rotation, Rotation())
numpy.testing.assert_array_equal(s.translation,
EigenArray.from_iterable((0, 0, 0)))
def test_new_default(self):
s = Similarity()
nose.tools.assert_equal(s.scale, 1)
numpy.testing.assert_array_almost_equal(s.rotation.matrix(),
Rotation().matrix())
numpy.testing.assert_array_equal(s.translation, [0, 0, 0])
def test_to_quaternion(self):
rot_d = Rotation('d')
numpy.testing.assert_array_equal(rot_d.quaternion(), [0, 0, 0, 1])
rot_f = Rotation('f')
numpy.testing.assert_array_equal(rot_f.quaternion(), [0, 0, 0, 1])
def test_compose(self):
# Normalize quaternaion vector.
expected_quat = array_normalize([+2., -1., -3., +0.])
r_ident_d = Rotation('d')
r_ident_f = Rotation('f')
r_other_d = Rotation.from_quaternion(expected_quat, 'd')
r_other_f = Rotation.from_quaternion(expected_quat, 'f')
r_res_d = r_ident_d.compose(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.compose(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)
# Should also work with multiply operator
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)
# Rotation of non-congruent types should be converted automatically
r_res_d = r_ident_d.compose(r_other_f)
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_allclose(r_res_d.quaternion(),
r_other_f.quaternion(), 1e-7)
numpy.testing.assert_allclose(r_res_d.quaternion(), expected_quat,
1e-7)
r_res_f = r_ident_f.compose(r_other_d)
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_allclose(r_res_f.quaternion(),
r_other_f.quaternion(), 1e-7)
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat,
1e-7)
# Equality check between types should pass due to integrety resolution
# inside function.
r_res_d = r_ident_d * r_other_f
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_allclose(r_res_d.quaternion(),
r_other_f.quaternion(), 1e-7)
numpy.testing.assert_allclose(r_res_d.quaternion(), expected_quat,
1e-7)
r_res_f = r_ident_f * r_other_d
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat,
1e-7)
