def test_rp(self): r = rotation.exp([.1, .2, .3]) p = np.array([1., 2., 3.]) v = SE3(r, p) rr, pp = v.rp assert_arrays_almost_equal(r, rr) assert_arrays_almost_equal(p, pp)
def test_rt(self): r = rotation.exp([.1, .2, .3]) t = np.array([1., 2., 3.]) v = SE3.from_matrix(np.hstack((r, t[:, None]))) rr, tt = v.rt assert_arrays_almost_equal(r, rr) assert_arrays_almost_equal(t, tt)
def test_skew(self): a = [1, 2, 3] expected = [ [0, -3, 2], [3, 0, -1], [-2, 1, 0]] assert_arrays_almost_equal(arithmetic.skew(a), expected)
def test_angular_velocity_from_axisangle_rates_at_zero(self): np.random.seed(0) # create three splines timestamps = np.linspace(0, 10, 10) controls = np.zeros((3, len(timestamps))) fs = [InterpolatedUnivariateSpline(timestamps, xs) for xs in controls] t = 0 t0 = t - 1e-8 t1 = t + 1e-8 r = rotation.exp([f(t) for f in fs]) r0 = rotation.exp([f(t0) for f in fs]) r1 = rotation.exp([f(t1) for f in fs]) w_numerical_global = angular_velocity_global_numerical(r0, r1, t0-t1) w_numerical_local = angular_velocity_local_numerical(r0, r1, t0-t1) ff = np.array([f(t) for f in fs]) dfdt = np.array([f.derivative()(t) for f in fs]) w_analytic_global = rotation.angular_velocity_from_axisangle_rates(ff, dfdt) w_analytic_local = np.dot(r, w_analytic_global) assert_arrays_almost_equal(w_numerical_global, w_analytic_global, xlabel='numerical', ylabel='analytic') assert_arrays_almost_equal(w_numerical_local, w_analytic_local, xlabel='numerical', ylabel='analytic')
def test_triangulate_directional_relative_pair(): np.random.seed(0) poses = [SE3.identity(), SE3.from_tangent(np.random.randn(6)*.1)] point = np.random.randn(3) + [0, 0, 10] features = [pr(np.dot(pose.orientation, point - pose.position)) for pose in poses] estimated = triangulate_directional_relative_pair(features[0], features[1], poses[1]) assert_arrays_almost_equal(estimated, point)
def test_angular_velocity_from_axisangle_rates_at_zero(self): np.random.seed(0) # create three splines timestamps = np.linspace(0, 10, 10) controls = np.zeros((3, len(timestamps))) fs = [InterpolatedUnivariateSpline(timestamps, xs) for xs in controls] t = 0 t0 = t - 1e-8 t1 = t + 1e-8 r = rotation.exp([f(t) for f in fs]) r0 = rotation.exp([f(t0) for f in fs]) r1 = rotation.exp([f(t1) for f in fs]) w_numerical_global = angular_velocity_global_numerical(r0, r1, t0 - t1) w_numerical_local = angular_velocity_local_numerical(r0, r1, t0 - t1) ff = np.array([f(t) for f in fs]) dfdt = np.array([f.derivative()(t) for f in fs]) w_analytic_global = rotation.angular_velocity_from_axisangle_rates( ff, dfdt) w_analytic_local = np.dot(r, w_analytic_global) assert_arrays_almost_equal(w_numerical_global, w_analytic_global, xlabel='numerical', ylabel='analytic') assert_arrays_almost_equal(w_numerical_local, w_analytic_local, xlabel='numerical', ylabel='analytic')
def test_unreduce_2d(self): a = [[1, 2], [3, 4]] mask = [True, False, True] expected = [ [1, 0, 2], [0, 0, 0], [3, 0, 4]] assert_arrays_almost_equal(arithmetic.unreduce_2d(a, mask), expected)
def test_socp(): constraints = [ ConeConstraint(np.eye(2), np.zeros(2), np.zeros(2), 3.), ConeConstraint(np.eye(2), [2, 0], np.zeros(2), 3.) ] problem = ConeProblem([0., -1.], constraints) solution = solve(problem) assert_arrays_almost_equal(np.squeeze(solution['x']), [-1., 2.*np.sqrt(2)])
def check_depth_solver(f, num_frames, noise, decimals): features, poses, true_position = generate_features(num_frames, noise) base_feature = features[0] aux_features = features[1:] base_pose = poses[0] aux_poses = poses[1:] true_depth = np.linalg.norm(base_pose.transform(true_position)) estimated_depth = f(base_feature, base_pose, aux_features, aux_poses) assert_arrays_almost_equal(estimated_depth, true_depth, decimals)
def test_socp(): constraints = [ ConeConstraint(np.eye(2), np.zeros(2), np.zeros(2), 3.), ConeConstraint(np.eye(2), [2, 0], np.zeros(2), 3.) ] problem = ConeProblem([0., -1.], constraints) solution = solve(problem) assert_arrays_almost_equal(np.squeeze(solution['x']), [-1., 2. * np.sqrt(2)])
def test_displacement_left_jacobian_wrt_lhs(self): np.random.seed(0) r1 = rotation.exp(np.random.randn(3)) r2 = rotation.exp(np.random.randn(3)) j_numerical = numeric_jacobian(lambda r: rotation.displacement_left(r, r2), r1, atlas=rotation.RotationAtlas) j_analytic = rotation.displacement_left_jacobian_wrt_lhs(r1, r2) assert_arrays_almost_equal(j_numerical, j_analytic, xlabel='analytic', ylabel='numerical')
def test_triangulate_directional_relative_pair(): np.random.seed(0) poses = [SE3.identity(), SE3.from_tangent(np.random.randn(6) * .1)] point = np.random.randn(3) + [0, 0, 10] features = [ pr(np.dot(pose.orientation, point - pose.position)) for pose in poses ] estimated = triangulate_directional_relative_pair(features[0], features[1], poses[1]) assert_arrays_almost_equal(estimated, point)
def test_displacement_left_jacobian_wrt_lhs(self): np.random.seed(0) r1 = rotation.exp(np.random.randn(3)) r2 = rotation.exp(np.random.randn(3)) j_numerical = numeric_jacobian( lambda r: rotation.displacement_left(r, r2), r1, atlas=rotation.RotationAtlas) j_analytic = rotation.displacement_left_jacobian_wrt_lhs(r1, r2) assert_arrays_almost_equal(j_numerical, j_analytic, xlabel='analytic', ylabel='numerical')
def test_numeric_jacobian_custom_inou(self): f = lambda f: Foo(f.x ** 2) assert_arrays_almost_equal(numeric_jacobian(f, Foo(3.)), [[6.]])
def test_numeric_jacobian_vector_output(self): f = lambda x: np.array([x**2, 3.-x]) assert_arrays_almost_equal(numeric_jacobian(f, 3.), [[6.], [-1.]])
def test_numeric_jacobian_vector_inout(self): f = lambda x: np.array((x[0] * 2., x[1]**2)) assert_arrays_almost_equal(numeric_jacobian(f, [3., 4.]), [[2., 0.], [0., 8.]])
def test_min_med_max(self): a = [1, 2, 3, 4, 5] assert_arrays_almost_equal(arithmetic.minmedmax(a), [1, 3, 5])
def test_triangulate_infnorm_fixed(): np.random.seed(0) features, poses, point = generate_features(num_frames=5) estimated = triangulate_infnorm_fixed(features, poses, 1e-8) assert_arrays_almost_equal(estimated, point)
def test_unreduce(self): a = [1, 2] mask = [False, True, False, True, False] assert_arrays_almost_equal(arithmetic.unreduce(a, mask), [0, 1, 0, 2, 0])
def check_triangulation(f, num_frames, noise, decimals): features, poses, true_position = generate_features(num_frames, noise) estimated = f(features, poses) assert_arrays_almost_equal(estimated, true_position, decimals)
def test_inverse(self): r = SE3.from_tangent([.1, .2, .3, -1, -2, -3]) rinv = SE3(r.orientation.T, -np.dot(r.orientation, r.position)) assert_arrays_almost_equal(r.inverse().matrix, rinv.matrix)
def test_log(self): r = SE3.from_tangent([.1, .2, .3, -1, -2, -3]) assert_arrays_almost_equal(r.log(), [.1, .2, .3, -1, -2, -3])
def test_identity(self): r = SE3.identity() assert_arrays_almost_equal(r.matrix[:3, :3], np.eye(3)) assert_arrays_almost_equal(r.matrix[:3, 3], [0, 0, 0]) assert_arrays_almost_equal(r.matrix[3], [0, 0, 0, 1])
def test_from_tangent(self): r = SE3.from_tangent([.1, .2, .3, -1, -2, -3]) assert_arrays_almost_equal(r.orientation, rotation.exp([.1, .2, .3])) assert_arrays_almost_equal(r.position, [-1, -2, -3])
def test_inverse(self): r = SO3.from_tangent([.1, .2, .3]) assert_arrays_almost_equal(r.inverse().matrix, r.matrix.T)
def test_from_tangent(self): r = SO3.from_tangent([.1, .2, .3]) assert_arrays_almost_equal(r.matrix, rotation.exp([.1, .2, .3]))
def test_scalar_atlas(self): self.assertEqual(ScalarAtlas.perturb(3, [-5]), -2) assert_arrays_almost_equal(ScalarAtlas.displacement(2, 6), [4])
def test_from_matrix(self): r = rotation.exp([.1, .2, .3]) t = np.array([1., 2., 3.]) v = SE3.from_matrix(np.hstack((r, t[:, None]))) assert_arrays_almost_equal(v.orientation, r) assert_arrays_almost_equal(v.position, -np.dot(r, t))
def test_pr(self): a = [4., 6., 2.] assert_arrays_almost_equal(arithmetic.pr(a), [2., 3.])
def test_numeric_jacobian_scalar(self): f = lambda x: x**2 assert_arrays_almost_equal(numeric_jacobian(f, 3.), [[6.]])
def test_triangulate_directional_pair(): np.random.seed(0) features, poses, point = generate_features(num_frames=2) estimated = triangulate_directional_pair(features[0], features[1], poses[0], poses[1]) assert_arrays_almost_equal(estimated, point)
def test_identity(self): r = SO3.identity() assert_arrays_almost_equal(r.matrix, np.eye(3))
def test_numeric_jacobian_custom_output(self): f = lambda x: Foo(x**2) assert_arrays_almost_equal(numeric_jacobian(f, 3.), [[6.]])
def test_numeric_jacobian_custom_inou(self): f = lambda f: Foo(f.x**2) assert_arrays_almost_equal(numeric_jacobian(f, Foo(3.)), [[6.]])
def test_numeric_jacobian_vector_input(self): f = lambda x: x[0] * 2. assert_arrays_almost_equal(numeric_jacobian(f, [3., 4.]), [[2., 0.]])
def test_dots(self): a = np.eye(2) b = np.array([[1., 2.], [3., 4.]]) assert_arrays_almost_equal(arithmetic.dots(a, b, a), b)
def test_numeric_jacobian_custom_output(self): f = lambda x: Foo(x ** 2) assert_arrays_almost_equal(numeric_jacobian(f, 3.), [[6.]])
def test_skew(self): a = [1, 2, 3] expected = [[0, -3, 2], [3, 0, -1], [-2, 1, 0]] assert_arrays_almost_equal(arithmetic.skew(a), expected)
def test_cartesian_atlas(self): a = np.array([1, 2, 3]) assert_arrays_almost_equal(CartesianAtlas.perturb(a, [1, 0, -1]), [2, 2, 2]) assert_arrays_almost_equal(CartesianAtlas.displacement(a, [2, 2, 2]), [1, 0, -1])
def test_unpr(self): a = [1., 2.] assert_arrays_almost_equal(arithmetic.unpr(a), [1., 2., 1.])
def test_unreduce_2d(self): a = [[1, 2], [3, 4]] mask = [True, False, True] expected = [[1, 0, 2], [0, 0, 0], [3, 0, 4]] assert_arrays_almost_equal(arithmetic.unreduce_2d(a, mask), expected)
def test_sumsq(self): a = [1, 2] assert_arrays_almost_equal(arithmetic.sumsq(a), 5)
def test_normalized(self): a = [2., 0., 0.] assert_arrays_almost_equal(arithmetic.normalized(a), [1., 0., 0.])
def test_unit(self): assert_arrays_almost_equal(arithmetic.unit(1, 5), [0, 1, 0, 0, 0])
def test_numeric_jacobian_vector_output(self): f = lambda x: np.array([x**2, 3. - x]) assert_arrays_almost_equal(numeric_jacobian(f, 3.), [[6.], [-1.]])