def test_read_write_krtd_file(self):
# Use a random string filename to avoid name collision.
fname = 'temp_camera_test_read_write_krtd_file.txt'
try:
for _ in range(100):
c = (rand(3)*2-1)*100
center = EigenArray.from_array([c])
rotation = Rotation.from_quaternion(numpy.random.rand(4)*2-1)
intrinsics = CameraIntrinsics(10, (5, 5), 1.2, 0.5, [4, 5, 6])
c1 = Camera(center, rotation,
intrinsics)
c1.write_krtd_file(fname)
c2 = Camera.from_krtd_file(fname)
err = numpy.linalg.norm(c1.center-c2.center)
assert err < 1e-9, ''.join(['Centers are different by ',
str(err)])
c1.rotation.angle_from(c2.rotation) < 1e-12
attr = ['focal_length','aspect_ratio','principle_point','skew',
'dist_coeffs']
for att in attr:
v1 = numpy.array(getattr(c1.intrinsics,att))
v2 = numpy.array(getattr(c2.intrinsics,att))
err = numpy.linalg.norm(v1-v2)
assert err < 1e-8, ''.join(['Difference ',str(err),
' for attribute: ',att])
finally:
if os.path.isfile(fname):
os.remove(fname)
def to_dict(self):
"""
:return: Internal frame-number to cameras mapping
:rtype: dict[int, Camera]
"""
cm_get_map = self.VITAL_LIB['vital_camera_map_get_map']
cm_get_map.argtypes = [
self.c_ptr_type(),
ctypes.POINTER(ctypes.c_size_t),
ctypes.POINTER(ctypes.POINTER(ctypes.c_int64)),
ctypes.POINTER(ctypes.POINTER(Camera.c_ptr_type())),
VitalErrorHandle.c_ptr_type()
]
length = ctypes.c_size_t()
frame_numbers = ctypes.POINTER(ctypes.c_int64)()
cameras = ctypes.POINTER(Camera.c_ptr_type())()
with VitalErrorHandle() as eh:
cm_get_map(self, ctypes.byref(length), ctypes.byref(frame_numbers),
ctypes.byref(cameras), eh)
m = {}
for i in xrange(length.value):
# copy camera cptr so we don't
cptr = Camera.c_ptr_type()(cameras[i].contents)
m[frame_numbers[i]] = Camera(from_cptr=cptr)
# Free frame number and camera pointer arrays
free_ptr = self.VITAL_LIB['vital_free_pointer']
free_ptr(frame_numbers)
free_ptr(cameras)
return m
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_depth(self):
cam = Camera()
pos_pt = numpy.array([1, 0, 1])
neg_pt = numpy.array([0, 0, -100])
nose.tools.assert_equal(cam.depth(pos_pt), 1)
nose.tools.assert_equal(cam.depth(neg_pt), -100)
def test_depth(self):
cam = Camera()
pos_pt = numpy.array([1,0,1])
neg_pt = numpy.array([0,0,-100])
nose.tools.assert_equal(cam.depth(pos_pt), 1)
nose.tools.assert_equal(cam.depth(neg_pt), -100)
def test_depth(self):
cam = Camera()
pos_pt = [[1], [0], [1]]
neg_pt = [[0], [0], [-100]]
nose.tools.assert_equal(cam.depth(pos_pt), 1)
nose.tools.assert_equal(cam.depth(neg_pt), -100)
def test_asmatrix_default(self):
cam = Camera()
m_expected = numpy.matrix("1 0 0 0;"
"0 1 0 0;"
"0 0 1 0")
print cam.as_matrix()
print m_expected
numpy.testing.assert_array_equal(
cam.as_matrix(),
m_expected
)
def test_equal(self):
cam1 = Camera()
cam2 = Camera()
nose.tools.assert_equal(cam1, cam1)
nose.tools.assert_equal(cam1, cam2)
center = EigenArray.from_iterable([[1], [2], [3]])
rotation = Rotation.from_axis_angle([[0], [1], [0]], math.pi / 2.)
cam1 = Camera(center, rotation)
cam2 = Camera(center, rotation)
nose.tools.assert_equal(cam1, cam1)
nose.tools.assert_equal(cam1, cam2)
def test_depth(self):
cam = Camera()
pos_pt = [[1],
[0],
[1]]
neg_pt = [[0],
[0],
[-100]]
nose.tools.assert_equal(cam.depth(pos_pt), 1)
nose.tools.assert_equal(cam.depth(neg_pt), -100)
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_covariance_default(self):
cam = Camera()
# Should be default value of an identity matrix
numpy.testing.assert_array_equal(
cam.covariance.to_matrix(),
numpy.eye(3),
)
def test_center_initialized(self):
expected_c = EigenArray(3)
expected_c.get_matrix()[:] = [[1], [2], [3]]
cam = Camera(expected_c)
numpy.testing.assert_array_equal(
numpy.array([cam.center]).T, expected_c.get_matrix())
numpy.testing.assert_array_equal(cam.center, [1, 2, 3])
def test_rotation_default(self):
cam = Camera()
# Should be identity by default
numpy.testing.assert_array_equal(
cam.rotation.matrix(),
numpy.eye(3),
)
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 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 test_read_write_krtd_file(self):
# Use a random string filename to avoid name collision.
fname = 'temp_camera_test_read_write_krtd_file.txt'
try:
for _ in range(100):
c = (rand(3) * 2 - 1) * 100
center = EigenArray.from_iterable(c)
rotation = Rotation.random()
intrinsics = CameraIntrinsics(focal_length=rand(1) * 1e4,
principle_point=rand(2) * 1000,
aspect_ratio=rand(1),
skew=0.,
dist_coeffs=rand(3))
c1 = Camera(center=center,
rotation=rotation,
intrinsics=intrinsics)
c1.write_krtd_file(fname)
c2 = Camera.from_krtd_file(fname)
err = numpy.linalg.norm(c1.center - c2.center)
assert err < 1e-9, ''.join(
['Centers are different by ',
str(err)])
c1.rotation.angle_from(c2.rotation) < 1e-12
attr = [
'focal_length', 'aspect_ratio', 'principle_point', 'skew',
'dist_coeffs'
]
for att in attr:
v1 = numpy.array(getattr(c1.intrinsics, att))
v2 = numpy.array(getattr(c2.intrinsics, att))
err = numpy.linalg.norm(v1 - v2)
assert err < 1e-8, ''.join(
['Difference ',
str(err), ' for attribute: ', att])
finally:
if os.path.isfile(fname):
os.remove(fname)
def test_to_from_string(self):
cam = Camera()
cam_s = cam.as_string()
cam2 = Camera.from_string(cam_s)
print "Default camera string:\n%s" % cam_s
print "Default newcam string:\n%s" % cam2.as_string()
nose.tools.assert_equal(cam, cam2)
center = EigenArray.from_iterable([[1], [2], [3]])
rotation = Rotation.from_axis_angle([[0], [1], [0]], math.pi / 2.)
cam = Camera(center, rotation)
cam_s = cam.as_string()
cam2 = Camera.from_string(cam_s)
print "Custom camera string:\n%s" % cam_s
print "Custom newcam string:\n%s" % cam2.as_string()
nose.tools.assert_equal(cam, cam2)
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 * Rotation.from_rodrigues(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 = 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_to_from_string(self):
cam = Camera()
cam_s = cam.as_string()
cam2 = Camera.from_string(cam_s)
print "Default camera string:\n%s" % cam_s
print "Default newcam string:\n%s" % cam2.as_string()
nose.tools.assert_equal(cam, cam2)
center = EigenArray.from_iterable([[1],
[2],
[3]])
rotation = Rotation.from_axis_angle([[0], [1], [0]], math.pi / 2.)
cam = Camera(center, rotation)
cam_s = cam.as_string()
cam2 = Camera.from_string(cam_s)
print "Custom camera string:\n%s" % cam_s
print "Custom newcam string:\n%s" % cam2.as_string()
nose.tools.assert_equal(cam, cam2)
def test_size(self):
m = {0: Camera(), 1: Camera(), 5: Camera()}
cm = CameraMap(m)
nose.tools.assert_equal(cm.size, 3)
def test_rotation_initialized(self):
center = EigenArray.from_array([[1], [2], [3]])
r_expected = Rotation.from_axis_angle([0, 1, 0], math.pi / 8)
cam = Camera(center, r_expected)
nose.tools.assert_is_not(cam.rotation, r_expected)
nose.tools.assert_equal(cam.rotation, r_expected)
def test_rotation_initialized(self):
r_expected = Rotation.from_axis_angle([[0], [1], [0]], math.pi / 8)
cam = Camera(rotation=r_expected)
nose.tools.assert_is_not(cam.rotation, r_expected)
nose.tools.assert_equal(cam.rotation, r_expected)
def test_to_dict(self):
m = {0: Camera(), 1: Camera(), 5: Camera()}
cm = CameraMap(m)
m2 = cm.to_dict()
nose.tools.assert_equal(m, m2)
def test_asmatrix_default(self):
cam = Camera()
m_expected = numpy.matrix("1 0 0 0;" "0 1 0 0;" "0 0 1 0")
print cam.as_matrix()
print m_expected
numpy.testing.assert_array_equal(cam.as_matrix(), m_expected)
def test_center_default(self):
cam = Camera()
numpy.testing.assert_array_equal(cam.center, [[0], [0], [0]])
def test_translation_initialized(self):
center = EigenArray.from_iterable([[1], [2], [3]])
rotation = Rotation.from_axis_angle([[0], [1], [0]], math.pi / 2.)
cam = Camera(center, rotation)
numpy.testing.assert_array_equal(cam.translation, -(rotation * center))
def test_translation_default(self):
cam = Camera()
numpy.testing.assert_array_equal(cam.translation, [[0], [0], [0]])
def test_center_initialized(self):
expected_c = EigenArray(3)
expected_c[:] = [[1], [2], [3]]
cam = Camera(expected_c)
numpy.testing.assert_array_equal(cam.center, expected_c)
numpy.testing.assert_array_equal(cam.center, [[1], [2], [3]])
