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_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_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_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_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 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_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_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_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_identity(self):
r = SO3.identity()
assert_arrays_almost_equal(r.matrix, np.eye(3))
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_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_scalar(self): f = lambda x: x**2 assert_arrays_almost_equal(numeric_jacobian(f, 3.), [[6.]])
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_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_min_med_max(self):
a = [1, 2, 3, 4, 5]
assert_arrays_almost_equal(arithmetic.minmedmax(a), [1, 3, 5])
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_numeric_jacobian_vector_input(self):
f = lambda x: x[0] * 2.
assert_arrays_almost_equal(numeric_jacobian(f, [3., 4.]), [[2., 0.]])
def test_unpr(self):
a = [1., 2.]
assert_arrays_almost_equal(arithmetic.unpr(a), [1., 2., 1.])
def test_scalar_atlas(self):
self.assertEqual(ScalarAtlas.perturb(3, [-5]), -2)
assert_arrays_almost_equal(ScalarAtlas.displacement(2, 6), [4])
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 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_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_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_normalized(self):
a = [2., 0., 0.]
assert_arrays_almost_equal(arithmetic.normalized(a), [1., 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.]])
