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 __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 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 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 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 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 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 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)