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_dynamic_size_init(self): a = EigenArray(2, dynamic_rows=True).get_matrix() ntools.assert_equal(a.shape, (2, 1)) a = EigenArray(300, dynamic_rows=True).get_matrix() ntools.assert_equal(a.shape, (300, 1)) a = EigenArray(1234, 256, dynamic_rows=True, dynamic_cols=True).get_matrix() ntools.assert_equal(a.shape, (1234, 256))
def test_mutability(self): a = EigenArray(2, 3) d = a.get_matrix() # The data pointer for i in range(2): for j in range(3): d[i][j] = 0 numpy.testing.assert_array_equal(a.get_matrix(), [[0, 0, 0], [0, 0, 0]]) d[:] = 1 numpy.testing.assert_array_equal(a.get_matrix(), [[1, 1, 1], [1, 1, 1]]) d[1, 0] = 2 numpy.testing.assert_array_equal(a.get_matrix(), [[1, 1, 1], [2, 1, 1]]) d[:, 2] = 3 numpy.testing.assert_array_equal(a.get_matrix(), [[1, 1, 3], [2, 1, 3]]) d += 1 numpy.testing.assert_array_equal(a.get_matrix(), [[2, 2, 4], [3, 2, 4]]) b = d * 0 numpy.testing.assert_array_equal(a.get_matrix(), [[2, 2, 4], [3, 2, 4]]) numpy.testing.assert_array_equal(b, [[0, 0, 0], [0, 0, 0]])
def test_from_array_1D(self): # 1-dim iterables/vectors are treated as column vectors input = [[1], [2], [3], [4]] expected = [[1], [2], [3], [4]] e = EigenArray.from_array(input) em = e.get_matrix() numpy.testing.assert_equal(em, expected) e2 = EigenArray.from_array(em) em2 = e2.get_matrix() numpy.testing.assert_equal(em, em2)
def test_matrix_init(self): # Test that construction does not fail when passing valid matrices as # initializer m1 = [[0, 1, 3], [0.3, 0.1, 10], [-1, 8.1, 4.7]] m2_d = EigenArray.from_array(m1, 'd') m2_f = EigenArray.from_array(m1, 'f') Homography.from_matrix(m1, 'd') Homography.from_matrix(m1, 'f') Homography.from_matrix(m2_d.get_matrix(), 'd') Homography.from_matrix(m2_d.get_matrix(), 'f') Homography.from_matrix(m2_f.get_matrix(), 'd') Homography.from_matrix(m2_f.get_matrix(), 'f')
def test_point_map(self): h_f = Homography('f') h_d = Homography('d') p_af = EigenArray.from_array([[2.2, 3.3]], 'f') p_f = p_af.get_matrix()[0] p_ad = EigenArray.from_array([[5.5, 6.6]], 'd') p_d = p_ad.get_matrix()[0] # float-float numpy.testing.assert_almost_equal(h_f.map(p_f), p_f) # float-double numpy.testing.assert_almost_equal(h_f.map(p_d), p_d) # double-float numpy.testing.assert_almost_equal(h_d.map(p_f), p_f) # double-double numpy.testing.assert_almost_equal(h_d.map(p_d), p_d) # Code to generate truth h = numpy.random.rand(3, 3) h = h / numpy.linalg.norm(h) p0 = numpy.random.rand(3) p0[2] = 1 p1 = numpy.dot(h, p0) p1 = p1[:2] / p1[2] h_d = Homography.from_matrix(h, 'd') # map from Numpy array. numpy.testing.assert_almost_equal(h_d.map(p0[:2]).ravel(), p1) # map from EigenArray p0 = EigenArray.from_array([p0[:2]]) numpy.testing.assert_almost_equal( h_d.map(p0.get_matrix()[0]).ravel(), p1) # Another explicit case. p0 = numpy.array([1923.47, 645.676, 1]) h = numpy.array( [[ 5.491496261770000276e-01, -1.125428185150000038e-01, 1.358427031619999923e+02 ], [ -1.429513389049999993e-02, 6.035527375529999849e-01, 5.923971959490000216e+01 ], [-2.042570000000000164e-06, -2.871670000000000197e-07, 1]]) p1 = numpy.dot(h, p0) p1 = p1[:2] / p1[2] H = Homography.from_matrix(h) P = EigenArray.from_array([p0[:2]]) numpy.testing.assert_almost_equal(H.map(P.get_matrix()[0]).ravel(), p1)
def test_valid_static_size_init(self): """ Test construction of some of the static sizes. """ a = EigenArray().get_matrix() # default shape ntools.assert_equal(a.shape, (2, 1)) a = EigenArray(2, 1).get_matrix() ntools.assert_equal(a.shape, (2, 1)) a = EigenArray(2, 2).get_matrix() ntools.assert_equal(a.shape, (2, 2)) a = EigenArray(4, 4).get_matrix() ntools.assert_equal(a.shape, (4, 4))
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_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_equal(self): cam1 = Camera() cam2 = Camera() nose.tools.assert_equal(cam1, cam1) nose.tools.assert_equal(cam1, cam2) center = EigenArray.from_array([[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_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_array([[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 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_order_transform(self): a = EigenArray(2, 3) d = a.get_matrix() # The data pointer # column-major 2x3 matrix [[ 1 2 3 ] (Eigen format) # [ 4 5 6 ]] d[0][0] = 1 d[0][1] = 2 d[0][2] = 3 d[1][0] = 4 d[1][1] = 5 d[1][2] = 6 numpy.testing.assert_array_equal(a.get_matrix(), [[1., 2., 3.], [4., 5., 6.]]) ntools.assert_equal(a.get_matrix()[0, 0], 1) ntools.assert_equal(a.get_matrix()[0, 1], 2) ntools.assert_equal(a.get_matrix()[0, 2], 3) ntools.assert_equal(a.get_matrix()[1, 0], 4) ntools.assert_equal(a.get_matrix()[1, 1], 5) ntools.assert_equal(a.get_matrix()[1, 2], 6)
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 test_from_array(self): # from list expected_list = [[0.4, 0], [1, 1.123], [2.253, 4.768124]] ea = EigenArray.from_array(expected_list) em = ea.get_matrix() numpy.testing.assert_array_equal(em, expected_list) # from ndarray expected_ndar = numpy.array(expected_list) ea = EigenArray.from_array(expected_ndar) em = ea.get_matrix() numpy.testing.assert_array_equal(em, expected_ndar) # from EigenArray, which should return the input object ea = EigenArray(3, 2) em = ea.get_matrix() em[:] = expected_list ea2 = EigenArray.from_array(em) em2 = ea2.get_matrix() numpy.testing.assert_array_equal(em2, em)
def test_new_matrix(self): a = EigenArray(2, 2, type='d') m = a.get_matrix() m[:] = 1. c = Covariance.from_matrix(2, 'd', m) m_out = c.to_matrix() print('input matrix:\n', m) print('output matrix:\n', m_out) numpy.testing.assert_array_equal(m_out, m) # Type casting should be handled a = EigenArray(2, 2, type='f') m = a.get_matrix() m[:] = 1. c = Covariance.from_matrix(2, 'd', m) m_out = c.to_matrix() print('input matrix:\n', m) print('output matrix:\n', m_out) numpy.testing.assert_array_equal(m_out, m) # Any other numpy array of the correct shape should be acceptable m = numpy.ndarray((2, 2)) m[:] = 3. c = Covariance.from_matrix(2, 'f', init=m) m_out = c.to_matrix() print('input matrix:\n', m) print('output matrix:\n', m_out) numpy.testing.assert_array_equal(m_out, m) # Diagonally congruent values should be averages when initializing with # matrix m = numpy.eye(3, dtype=numpy.double) m[0, 2] = 2. m_expected = m.copy() m_expected[0, 2] = 1. m_expected[2, 0] = 1. c = Covariance.from_matrix(3, init=m) m_out = c.to_matrix() print('input matrix:\n', m) print('output matrix:\n', m_out) numpy.testing.assert_array_equal(m_out, m_expected)
def test_norm(self): e = EigenArray.from_array([[1], [2], [3], [4]]) numpy.linalg.norm(e.get_matrix()) == numpy.sqrt(1 + 4 + 9 + 16)
def test_translation_initialized(self): center = EigenArray.from_array([[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.get_matrix()))
def random_point_3d(stddev): pt = [[numpy.random.normal(0., stddev)], [numpy.random.normal(0., stddev)], [numpy.random.normal(0., stddev)]] return EigenArray.from_iterable(pt, target_shape=(3, 1))
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)