def check_pi_test():
for g, w in known_pairs:
w2 = se2_from_SE2(g)
# printm('g', g, 'w', w, 'w2', w2)
assert_allclose(w, w2, atol=1e-8)
g2 = SE2_from_se2(w2)
assert_allclose(g, g2, atol=1e-8)
def best_orthogonal_transform_test2():
N = 20
X = random_directions(N)
Y = random_directions(N)
R1 = best_orthogonal_transform(X, Y)
R2 = best_orthogonal_transform(Y, X)
assert_allclose(R1.T, R2)
def comparison_test_2():
''' Compares between se2_from_SE2 and se2_from_SE2_slow. '''
for pose in SE2.interesting_points():
se2a = se2_from_SE2(pose)
se2b = se2_from_SE2_slow(pose)
#printm('pose', pose, 'se2a', se2a, 'se2b', se2b)
assert_allclose(se2a, se2b, atol=1e-8)
def comparison_test_2():
''' Compares between se2_from_SE2 and se2_from_SE2_slow. '''
for pose in SE2.interesting_points():
se2a = se2_from_SE2(pose)
se2b = se2_from_SE2_slow(pose)
#printm('pose', pose, 'se2a', se2a, 'se2b', se2b)
assert_allclose(se2a, se2b, atol=1e-8)
def check_pi_test():
for g, w in known_pairs:
w2 = se2_from_SE2(g)
# printm('g', g, 'w', w, 'w2', w2)
assert_allclose(w, w2, atol=1e-8)
g2 = SE2_from_se2(w2)
assert_allclose(g, g2, atol=1e-8)
def best_orthogonal_transform_test2():
N = 20
X = random_directions(N)
Y = random_directions(N)
R1 = best_orthogonal_transform(X, Y)
R2 = best_orthogonal_transform(Y, X)
assert_allclose(R1.T, R2)
def __init__(self, sigma, texture, resolution=None):
sigma = float(sigma)
if resolution is None:
resolution = sigma / 10.0
max_length = 100
texture = instantiate_spec(texture)
num_cells = max_length / resolution
cell_coord = np.linspace(0, max_length, num_cells)
unsmoothed = texture(cell_coord)
kernel_size = int(2 * (sigma * 3) / resolution)
#print('resol: %s' % resolution)
#print('sigma: %s' % sigma)
#print('kernel_size: %s' % kernel_size)
#print kernel
kernel = gaussian(kernel_size, sigma / resolution)
kernel = kernel / kernel.sum()
assert kernel.size == kernel_size
assert_allclose(kernel.sum(), 1)
smoothed = convolve(unsmoothed, kernel, 'same')
assert smoothed.size == cell_coord.size
SampledTexture.__init__(self, smoothed, resolution)
def test_se3_se2():
for pose in SE2.interesting_points():
pose3 = SE3_from_SE2(pose)
vel3 = SE3.algebra_from_group(pose3)
vel2 = se2_from_se3(vel3)
pose2 = SE2.group_from_algebra(vel2)
assert_allclose(pose2, pose, atol=1e-8)
def rotation_from_axes_spec__test():
for x in directions_sequence():
v = any_distant_direction(x)
R = rotation_from_axes_spec(x, v)
x_ = np.dot(R, x)
assert_allclose(x_, [1, 0, 0], atol=1e-8)
v_ = np.dot(R, v)
assert_allclose(v_[2], 0, atol=1e-8)
def hat_map_test():
for s in directions_sequence():
for v in directions_sequence():
x1 = np.cross(s, v)
x2 = +np.dot(hat_map(s), v)
x3 = -np.dot(hat_map(v), s)
assert_allclose(x1, x2)
assert_allclose(x1, x3)
def euclidean_distances_test():
n = 5
P = np.random.rand(3, n)
D = euclidean_distances(P)
assert D.shape == (n, n)
for i, j in itertools.product(range(n), range(n)):
d = np.linalg.norm(P[:, i] - P[:, j])
assert_allclose(d, D[i, j])
def hat_map_test():
for s in directions_sequence():
for v in directions_sequence():
x1 = np.cross(s, v)
x2 = +np.dot(hat_map(s), v)
x3 = -np.dot(hat_map(v), s)
assert_allclose(x1, x2)
assert_allclose(x1, x3)
def mds_test():
for n in [10, 100]:
for k in [3, 4, 5]:
P = np.random.rand(k, n)
D = euclidean_distances(P)
P2 = mds(D, ndim=k)
error = evaluate_error(P, P2)
assert_allclose(0, error, atol=1e-7)
def rotation_from_axes_spec__test():
for x in directions_sequence():
v = any_distant_direction(x)
R = rotation_from_axes_spec(x, v)
x_ = np.dot(R, x)
assert_allclose(x_, [1, 0, 0], atol=1e-8)
v_ = np.dot(R, v)
assert_allclose(v_[2], 0, atol=1e-8)
def mds_test():
for n in [10, 100]:
for k in [3, 4, 5]:
P = np.random.rand(k, n)
D = euclidean_distances(P)
P2 = mds(D, ndim=k)
error = evaluate_error(P, P2)
assert_allclose(0, error, atol=1e-7)
def euclidean_distances_test():
n = 5
P = np.random.rand(3, n)
D = euclidean_distances(P)
assert D.shape == (n, n)
for i, j in itertools.product(range(n), range(n)):
d = np.linalg.norm(P[:, i] - P[:, j])
assert_allclose(d, D[i, j])
def place_test():
for n in [4, 10]:
for k in [3]:
S = np.random.rand(k, n)
p = np.random.rand(k)
ref = lambda x: np.linalg.norm(p - x)
distances = np.array([ref(S[:, i]) for i in range(n)])
p2 = place(S, distances)
assert_allclose(p, p2)
def test_slerp(self):
for r1, r2 in itertools.product(rotations_sequence(),
rotations_sequence()):
q1 = quaternion_from_rotation(r1)
q2 = quaternion_from_rotation(r2)
for t in [0, 0.1, 0.5, 0.75, 1]:
a = slerp(q1, q2, t)
b = slerp(q2, q1, 1 - t)
assert_allclose(a, b)
def place_test():
for n in [4, 10]:
for k in [3]:
S = np.random.rand(k, n)
p = np.random.rand(k)
ref = lambda x: np.linalg.norm(p - x)
distances = np.array([ref(S[:, i]) for i in range(n)])
p2 = place(S, distances)
assert_allclose(p, p2)
def test_slerp(self):
for r1, r2 in itertools.product(rotations_sequence(),
rotations_sequence()):
q1 = quaternion_from_rotation(r1)
q2 = quaternion_from_rotation(r2)
for t in [0, 0.1, 0.5, 0.75, 1]:
a = slerp(q1, q2, t)
b = slerp(q2, q1, 1 - t)
assert_allclose(a, b)
def rank_test():
''' Check that the double-centered matrix has small rank. '''
for n in range(5, 50, 5):
for k in range(1, 5):
P = np.random.rand(k, n)
D = euclidean_distances(P)
B = double_center(D * D)
w, v = eigh(B) #@UnusedVariable
w = w[::-1] # descending
# normalize
wn = w / w[0]
small = np.abs(wn[k])
assert_allclose(0, small, atol=1e-7)
def test_rotation_from_axis_angle2(self):
for axis, angle in axis_angle_sequence():
R1 = rotation_from_axis_angle(axis, angle)
R2 = rotation_from_axis_angle2(axis, angle)
if False:
s1, a1 = axis_angle_from_rotation(R1)
s2, a2 = axis_angle_from_rotation(R2)
print('Origi: %s around %s' % (angle, axis))
print('First: %s around %s' % (a1, s1))
print('Secnd: %s around %s' % (a2, s2))
assert_allclose(R1, R2)
def test_rotation_from_axis_angle2(self):
for axis, angle in axis_angle_sequence():
R1 = rotation_from_axis_angle(axis, angle)
R2 = rotation_from_axis_angle2(axis, angle)
if False:
s1, a1 = axis_angle_from_rotation(R1)
s2, a2 = axis_angle_from_rotation(R2)
print('Origi: %s around %s' % (angle, axis))
print('First: %s around %s' % (a1, s1))
print('Secnd: %s around %s' % (a2, s2))
assert_allclose(R1, R2)
def rank_test():
''' Check that the double-centered matrix has small rank. '''
for n in range(5, 50, 5):
for k in range(1, 5):
P = np.random.rand(k, n)
D = euclidean_distances(P)
B = double_center(D * D)
w, v = eigh(B) #@UnusedVariable
w = w[::-1] # descending
# normalize
wn = w / w[0]
small = np.abs(wn[k])
assert_allclose(0, small, atol=1e-7)
def distancetree_test1():
points, D = get_domain(1, 10)
order = make_distancesequence(D)
for x in order:
print('point: ', points[x])
assert_allclose(points[order[0]], (0, 4))
assert_allclose(points[order[1]], (0, 7))
order, G = make_distancetree(D)
filename = 'out/distancetree_test1'
draw_graph(G, points, filename)
def poses2markers(poses, scale=1):
Srel = np.zeros((3, 3))
H = np.sqrt(3) / 4
A = np.array([+H , 0])
B = np.array([-H , +0.5])
C = np.array([-H , -0.5])
mean = (A + B + C) / 3
A -= mean
B -= mean
C -= mean
A *= scale
B *= scale
C *= scale
# Make sure it is an equilateral triangle
# of side "scale"
dist = lambda x, y: np.linalg.norm(x - y)
assert_allclose(dist(A, B), dist(B, C))
assert_allclose(dist(B, C), dist(A, C))
assert_allclose(dist(A, B), scale)
assert_allclose((A + B + C) / 3, [0, 0])
Srel[:, 0] = [A[0], A[1], 1]
Srel[:, 1] = [B[0], B[1], 1]
Srel[:, 2] = [C[0], C[1], 1]
N = len(poses)
S = np.zeros((2, 3 * N))
for i, pose in enumerate(poses):
markers = np.dot(pose, Srel)[:2, :]
S[:, i + 0 * N] = markers[:, 0]
S[:, i + 1 * N] = markers[:, 1]
S[:, i + 2 * N] = markers[:, 2]
return S
def mds_fast_test():
for n in [10, 100]:
for k in [2, 3]:
P = np.random.rand(k, n)
D = euclidean_distances(P)
for algo in [mds, mds_randomized]:
# t0 = time.clock()
P2 = algo(D, ndim=k)
# t1 = time.clock()
#t_mds = t1 - t0
# D2 = euclidean_distances(P2)
error = evaluate_error(P, P2)
assert_allclose(0, error, atol=1e-7)
def mds_fast_test():
for n in [10, 100]:
for k in [2, 3]:
P = np.random.rand(k, n)
D = euclidean_distances(P)
for algo in [mds, mds_randomized]:
# t0 = time.clock()
P2 = algo(D, ndim=k)
# t1 = time.clock()
#t_mds = t1 - t0
# D2 = euclidean_distances(P2)
error = evaluate_error(P, P2)
assert_allclose(0, error, atol=1e-7)
def check_one(self, x, op1, op2):
def call(function, param):
if isinstance(param, tuple):
return function(*param)
else:
return function(param)
y = call(op1, x)
x2 = call(op2, y)
if isinstance(x, tuple):
for a, b in zip(x, x2):
assert_allclose(a, b)
else:
assert_allclose(x, x2)
def test_circle_circle_intersection(self):
for function, params, expected in CollisionUtilsTest.all_tests:
result = function(*params)
err_msg = 'Invalid match for %s:%s' % (function.__name__,
params.__repr__())
err_msg += '\n expected: %s' % expected.__repr__()
err_msg += '\n obtained: %s' % result.__repr__()
if expected is None:
self.assertEqual(expected, result, msg=err_msg)
else:
if result is None:
raise Exception(err_msg)
assert_allclose(expected[0], result[0], atol=1e-8,
err_msg=err_msg)
assert_allclose(expected[1], result[1], err_msg=err_msg)
def check_one(self, x, op1, op2):
def call(function, param):
if isinstance(param, tuple):
return function(*param)
else:
return function(param)
y = call(op1, x)
x2 = call(op2, y)
if isinstance(x, tuple):
for a, b in zip(x, x2):
assert_allclose(a, b)
else:
assert_allclose(x, x2)
def test_circle_circle_intersection(self):
for function, params, expected in CollisionUtilsTest.all_tests:
result = function(*params)
err_msg = 'Invalid match for %s:%s' % (function.__name__,
params.__repr__())
err_msg += '\n expected: %s' % expected.__repr__()
err_msg += '\n obtained: %s' % result.__repr__()
if expected is None:
self.assertEqual(expected, result, msg=err_msg)
else:
if result is None:
raise Exception(err_msg)
assert_allclose(expected[0],
result[0],
atol=1e-8,
err_msg=err_msg)
assert_allclose(expected[1], result[1], err_msg=err_msg)
def comparison_test():
''' Compares between SE2_from_se2_slow and SE2_from_se2. '''
for pose in SE2.interesting_points():
se2 = se2_from_SE2(pose)
SE2a = SE2_from_se2_slow(se2)
SE2b = SE2_from_se2(se2)
#printm('pose', pose, 'se2', se2)
#printm('SE2a', SE2a, 'SE2b', SE2b)
SE2.assert_close(SE2a, pose)
#print('SE2a = pose Their distance is %f' % d)
SE2.assert_close(SE2b, pose)
#print('SE2b = pose Their distance is %f' % d)
assert_allclose(SE2a, SE2b, atol=1e-8, err_msg='SE2a != SE2b')
assert_allclose(SE2a, pose, atol=1e-8, err_msg='SE2a != pose')
assert_allclose(SE2b, pose, atol=1e-8, err_msg='SE2b != pose')
def comparison_test():
''' Compares between SE2_from_se2_slow and SE2_from_se2. '''
for pose in SE2.interesting_points():
se2 = se2_from_SE2(pose)
SE2a = SE2_from_se2_slow(se2)
SE2b = SE2_from_se2(se2)
#printm('pose', pose, 'se2', se2)
#printm('SE2a', SE2a, 'SE2b', SE2b)
SE2.assert_close(SE2a, pose)
#print('SE2a = pose Their distance is %f' % d)
SE2.assert_close(SE2b, pose)
#print('SE2b = pose Their distance is %f' % d)
assert_allclose(SE2a, SE2b, atol=1e-8, err_msg='SE2a != SE2b')
assert_allclose(SE2a, pose, atol=1e-8, err_msg='SE2a != pose')
assert_allclose(SE2b, pose, atol=1e-8, err_msg='SE2b != pose')
def best_similarity_transform_test():
N = 20
for K in [3]: # TODO: multiple dimensions
X = np.random.randn(K, N)
R = random_rotation()
# R = np.eye(K)
t = np.random.randn(K, 1)
# print 'R: %s' % R
# print 't: %s' % t
Y = np.dot(R, X) + t
R2, t2 = best_similarity_transform(X, Y)
# print 'R2: %s' % R2
# print 't2: %s' % t2
Y2 = np.dot(R2, X) + t2
assert_allclose(R, R2, atol=1e-10)
assert_allclose(t, t2)
assert_allclose(Y, Y2)
def best_similarity_transform_test():
N = 20
for K in [3]: # TODO: multiple dimensions
X = np.random.randn(K, N)
R = random_rotation()
# R = np.eye(K)
t = np.random.randn(K, 1)
# print 'R: %s' % R
# print 't: %s' % t
Y = np.dot(R, X) + t
R2, t2 = best_similarity_transform(X, Y)
# print 'R2: %s' % R2
# print 't2: %s' % t2
Y2 = np.dot(R2, X) + t2
assert_allclose(R, R2, atol=1e-10)
assert_allclose(t, t2)
assert_allclose(Y, Y2)
def get_uniform_directions(fov_deg, num_sensels):
""" Returns a set of directions uniform in space """
if fov_deg == 360:
ray_dist = 2 * np.pi / (num_sensels)
directions = np.linspace(-np.pi + ray_dist / 2,
+np.pi - ray_dist + ray_dist / 2, num_sensels)
assert_allclose(directions[-1] - directions[0], 2 * np.pi - ray_dist)
t = np.rad2deg(directions)
a = t[1:] - t[:-1]
b = t[0] - t[-1] + 360
assert_allclose(a[0], b)
else:
fov_rad = np.radians(fov_deg)
directions = np.linspace(-fov_rad / 2, +fov_rad / 2, num_sensels)
assert_allclose(directions[-1] - directions[0], fov_rad)
assert len(directions) == num_sensels
return directions
def get_uniform_directions(fov_deg, num_sensels):
""" Returns a set of directions uniform in space """
if fov_deg == 360:
ray_dist = 2 * np.pi / (num_sensels)
directions = np.linspace(-np.pi + ray_dist / 2,
+ np.pi - ray_dist + ray_dist / 2,
num_sensels)
assert_allclose(directions[-1] - directions[0], 2 * np.pi - ray_dist)
t = np.rad2deg(directions)
a = t[1:] - t[:-1]
b = t[0] - t[-1] + 360
assert_allclose(a[0], b)
else:
fov_rad = np.radians(fov_deg)
directions = np.linspace(-fov_rad / 2, +fov_rad / 2, num_sensels)
assert_allclose(directions[-1] - directions[0], fov_rad)
assert len(directions) == num_sensels
return directions
def closest_orthogonal_matrix_test1():
R = random_rotation()
R2 = closest_orthogonal_matrix(R)
assert_allclose(R, R2)
def test_distances(self):
for i in range(N): #@UnusedVariable
s = random_direction()
dist = geodesic_distance_on_sphere
assert_allclose(dist(s, s), 0)
assert_allclose(dist(s, -s), np.pi)
def best_orthogonal_transform_test1():
X = random_directions(20)
R = random_rotation()
Y = np.dot(R, X)
R2 = best_orthogonal_transform(X, Y)
assert_allclose(R, R2)
def best_orthogonal_transform_test1():
X = random_directions(20)
R = random_rotation()
Y = np.dot(R, X)
R2 = best_orthogonal_transform(X, Y)
assert_allclose(R, R2)
def closest_orthogonal_matrix_test1():
R = random_rotation()
R2 = closest_orthogonal_matrix(R)
assert_allclose(R, R2)
def test_distances(self):
for i in range(N): # @UnusedVariable
s = random_direction()
dist = geodesic_distance_on_sphere
assert_allclose(dist(s, s), 0)
assert_allclose(dist(s, -s), np.pi)
