def test_init_with_list_dim_3(self): tree = KDTree(3, [(1, 2, 3), (0, 1, 4), (2, 4, 3)]) assert tree.root.data == (1, 2, 3) assert tree.root.left.data == (0, 1, 4) assert tree.root.right.data == (2, 4, 3) assert tree.size == 3 assert tree.is_empty() is False
def grow(self): recovery_indx_table = self.__add_ghost_boundary() coords_pair_lst = self.__gen_coords_pair_lst() stree = KDTree(coords_pair_lst) #stree = spatial.KDTree(coords_pair_lst) #print stree.data return recovery_indx_table, stree
def get_closest_waypoint(self, pose): """Identifies the closest path waypoint to the given position https://en.wikipedia.org/wiki/Closest_pair_of_points_problem Args: pose (Pose): position to match a waypoint to Returns: int: index of the closest waypoint in self.waypoints """ if self.waypoints is not None and self.kdtree is None: if VERBOSE: print('tl_detector: g_cl_wp: initializing kdtree') points = [] for i, waypoint in enumerate(self.waypoints): points.append((float(waypoint.pose.pose.position.x), float(waypoint.pose.pose.position.y), i)) self.kdtree = KDTree(points) if self.kdtree is not None: current_position = (pose.position.x, pose.position.y) closest = self.kdtree.closest_point(current_position) if VERBOSE: print('tl_detector: g_cl_wp: closest point to {} is {}'.format( current_position, closest)) return closest[2] return 0
def test_init_with_list_dim_2(self): tree = KDTree(2, [(1, 1), (3, 3), (2, 2)]) assert tree.root.data == (1, 1) assert tree.root.left == None assert tree.root.right.data == (3, 3) assert tree.root.right.left.data == (2, 2) assert tree.size == 3 assert tree.is_empty() is False
def test_size(self): tree = KDTree(1) assert tree.size == 0 tree.insert('B') assert tree.size == 1 tree.insert('A') assert tree.size == 2 tree.insert('C') assert tree.size == 3
def __init__(self, p_set, measurer=SquareDistMeasurer(2)): super(KDFinder, self).__init__(None, measurer) assert p_set is not None self.count = len(p_set) assert self.count > 0 self.tree = KDTree(None, 'value', measurer.k) for i in range(len(p_set)): self.tree.insert(Element(p_set[i], i)) self.debug = False
def __init__(self, world): self.points = KDTree(2, [world.kdtreeStart]) # number of samples in the prm self.size = 2500 # number of connections per sample self.connsPerSample = 6 self.carSize = world.carSize self.getPoints(world) # edges of the prm self.connections = self.getConnections(world)
def __init__(self, start, goal): self.start = start self.goal = goal # distance within which tree has 'reached' the goal self.eps = 5 # extension distance self.ext = 10 self.tree = KDTree(len(self.start), [self.start]) # edges self.connections = dict()
def test_init_with_larger_list_dim_5(self): tree = KDTree(5, [(1, 2, 3, 4, 5), (0, 1, 4, 1, 2), (2, 4, 3, 6, 7), (9, 8, 10, 7, 3), (-1, 0, 0, 14, 15)]) assert tree.root.data == (1, 2, 3, 4, 5) assert tree.root.left.data == (0, 1, 4, 1, 2) assert tree.root.right.data == (2, 4, 3, 6, 7) assert tree.root.right.right.data == (9, 8, 10, 7, 3) assert tree.root.left.left.data == (-1, 0, 0, 14, 15) assert tree.size == 5 assert tree.is_empty() is False
def test_nearest_neighbors(self): tree = KDTree(2, [(1, 1), (2, 2), (4, 4)]) neighbors = tree.nearest_neighbors((0, 0)) assert neighbors[0] == ((1, 1), 1.4142135623730951) assert neighbors[1] == ((2, 2), 2.8284271247461903) assert neighbors[2] == ((4, 4), 5.656854249492381) neighbors2 = tree.nearest_neighbors((1, 1)) assert neighbors2[0] == ((1, 1), 0) assert neighbors2[1] == ((2, 2), 1.4142135623730951) assert neighbors2[2] == ((4, 4), 4.242640687119285)
def fit(self, X, y): """Fit the model (build tree based on X). Args: X (array-like, shape (n_samples, n_features)): Training data. y (array, shape (n_samples,)): Target values. """ self.X, self.y = np.array(X), np.array(y) if self.algorithm == 'kd_tree': self.tree = KDTree(X, self.leaf_size, self.p) if self.algorithm == 'ball_tree': pass
def setUp(self): self.dim = 3 self.count = 20 points = np.random.randint(low=0, high=20, size=(self.count, self.dim)).tolist() self.tree = KDTree(self.dim) for p in points: self.tree.insert(p) np.random.shuffle(points) self.points = points
def planning(self, sx, sy, gx, gy, ox, oy, robot_radius): obstacle_tree = KDTree(np.vstack((ox, oy)).T) sample_x, sample_y = self.voronoi_sampling(sx, sy, gx, gy, ox, oy) if show_animation: # pragma: no cover plt.plot(sample_x, sample_y, ".b") road_map_info = self.generate_road_map_info( sample_x, sample_y, robot_radius, obstacle_tree) rx, ry = DijkstraSearch(show_animation).search(sx, sy, gx, gy, sample_x, sample_y, road_map_info) return rx, ry
def test_kdtree(self): tree = KDTree() tree.insert((2, 6)) tree.insert((3, 1)) tree.insert((8, 7)) tree.insert((10, 2)) tree.insert((13, 3)) assert tree.get_nearest((9, 4)) == ((10, 2), 2.23606797749979) assert tree.get_nearest((4, 1.5))[0] == (3, 1) assert tree.get_nearest((7, 8))[0] == (8, 7) assert tree.get_nearest((11, 1))[0] == (10, 2) assert tree.get_nearest((13, 3))[0] == (13, 3)
def fitICPkdTree(model, target): Tf = np.eye(4, 4) dif = 100 nIter = 0 kdTree = KDTree(list(target)) while nIter < MAX_ITER and dif > 10: T1, pit1 = ICPstepKDTree(model, kdTree) Tf = Tf.dot(T1) saDif = vector(sum(abs(model - pit1))) dif = saDif.mag #difference with respect to the anterior model print nIter, dif #points(pos=pit1,color=(0,1,1)) model = pit1 nIter += 1 #print nIter,dif return Tf, pit1
def __init__(self, start, goal, llim, ulim, robot_radius, obstacles, dim=2): """ start: obstacles: Only rectangular for now. np.array of (x,y) positions defining a set of obstacles """ self._dim = dim start, goal = np.array(start), np.array(goal) self._check_last_axis(start, "start") self._check_last_axis(goal, "goal") self._obstacle_config = obstacles self._obstacles = self.create_obstacles(obstacles) self._obs_kdtree = KDTree(self._obstacles) self._robot_radius = robot_radius self.llim = np.array(llim) self.ulim = np.array(ulim) self.start = start self.goal = goal
def generate_road_map_info(self, node_x, node_y, rr, obstacle_tree): """ Road map generation node_x: [m] x positions of sampled points node_y: [m] y positions of sampled points rr: Robot Radius[m] obstacle_tree: KDTree object of obstacles """ road_map = [] n_sample = len(node_x) node_tree = KDTree(np.vstack((node_x, node_y)).T) for (i, ix, iy) in zip(range(n_sample), node_x, node_y): index, dists = node_tree.search( np.array([ix, iy]).reshape(2, 1), k=n_sample) inds = index[0] edge_id = [] for ii in range(1, len(inds)): nx = node_x[inds[ii]] ny = node_y[inds[ii]] if not self.is_collision(ix, iy, nx, ny, rr, obstacle_tree): edge_id.append(inds[ii]) if len(edge_id) >= self.N_KNN: break road_map.append(edge_id) # plot_road_map(road_map, sample_x, sample_y) return road_map
def merge_graph(self, ): """ merge_graph and reassign work on queue """ work = self._graph_queue.get() if work: work = WorkMessage.deserialize(work.decode('utf-8')) id = (tuple(work.llim), tuple(work.ulim)) curr_graph = self._graph_by_division[id] curr_graph = nx.compose(curr_graph, work.networkx_graph) self._overall_graph = nx.compose(self._overall_graph, work.networkx_graph) overall_nodelist = list(self._overall_graph.nodes) overall_kdtree = KDTree(overall_nodelist) required_vertices = [] for vertex in self._scenario.get_vertices(work.llim, work.ulim): idxs = overall_kdtree.search_in_distance(vertex, 20) required_vertices.extend([overall_nodelist[i] for i in idxs]) graph_to_be_sent = self._overall_graph.subgraph(required_vertices) curr_graph = nx.compose(curr_graph, graph_to_be_sent) curr_message = WorkMessage(work.llim, work.ulim, curr_graph) self._worker_queue.put(curr_message.serialize()) self._graph_by_division[id] = curr_graph
def fit(self, X, y, k_neighbors=3): self.k_neighbors = k_neighbors self.tree = KDTree() self.tree.build_tree(X, y)
# Imports from files in local directory: from kdtree import KDTree class ZipPoint(tuple): '''Tuple containing a lat, long, and zip code.''' def __new__(cls, json_dict): return super(ZipPoint, cls).__new__(cls, (json_dict['lat'], json_dict['lng'])) def __init__(self, json_dict): self.zip = json_dict['zip'] with open('zip_centers.json', 'r') as f: ZIP_INDEX = KDTree([ZipPoint(json.loads(r)) for r in f]) def apply(input): val = json.loads(input) closest = ZIP_INDEX.query((val['lat'], val['lng'])) if closest: val['zip'] = closest[0].zip yield json.dumps(val) + '\n' else: yield input if __name__ == '__main__': for line in sys.stdin: for o in apply(line):
self.callback.draw() class Visualizer: def __init__(self, points): self.scenes = [] self.points = points self.lines = [] l = self.lines[:] self.scenes.append(Scene([PointsCollection(self.points, color='green')], [LinesCollection(l, color='black')])) def add_line(self, p1, p2): line = [p1, p2] self.lines.append(line) l = self.lines[:] scene = Scene([PointsCollection(self.points, color='green')], [LinesCollection(l, color='black')]) self.scenes.append(scene) ######################## Wizualizacja tworzenia drzewa ######################################################### points4 = [(20, 50), (30, 40), (35, 60), (50, 100), (60, 70), (10, 45),(10,10),(45,23),(7,8),(1,3),(18,90), (80,80)] visualizer = Visualizer(points4) tree = KDTree(points4, visualizer) plot = Plot(visualizer.scenes) plot.draw() ####################### Wizaualizacja znajdowanych punktów #####################################################
#!/usr/bin/env python import fileinput import random from kdtree import KDTree # read in the points from a file specified on the command line. E.g.: # $ ./kdtree_test.py ../../../data/sim_waypoints.csv points = [] i=0 for line in fileinput.input(): line_parts = line.split(',') points.append((float(line_parts[0]), float(line_parts[1]), int(i))) i += 1 # generate the K-D Tree from the points kdtree = KDTree(points) # pick a random point from the points rand_index = random.randint(0, len(points)) # find the closest point point = points[rand_index] print ("randomly chose point {} at index {}".format(point, rand_index)) new_point = (point[0] + 2.0, point[1] + 7.0) print ("tweaked x,y to be {}".format(new_point, rand_index)) closest = kdtree.closest_point(new_point) print ("closest point to {} is {}".format(new_point, closest))
def main(data_dir): # kNN & RNN configuration: leaf_size = 32 min_extent = 0.0001 k = 8 radius = 1 # kitti velodyne cat = os.listdir(data_dir) iteration_num = len(cat) print("OCTree --------------") construction_time_sum = 0 knn_time_sum = 0 radius_time_sum = 0 brute_time_sum = 0 for i in range(iteration_num): filename = os.path.join(data_dir, cat[i]) point_cloud = read_velodyne_bin(filename) # build tree: begin_t = time.time() octree = OCTree(point_cloud=point_cloud, leaf_size=leaf_size, min_extent=min_extent) construction_time_sum += time.time() - begin_t query = point_cloud[0, :] # kNN query: begin_t = time.time() knn_result_set = KNNResultSet(capacity=k) octree.knn_search(query, knn_result_set) knn_time_sum += time.time() - begin_t # RNN query: begin_t = time.time() rnn_result_set = RadiusNNResultSet(radius=radius) octree.rnn_fast_search(query, rnn_result_set) radius_time_sum += time.time() - begin_t # brute force: begin_t = time.time() diff = np.linalg.norm(point_cloud - query, axis=1) nn_idx = np.argsort(diff) nn_dist = diff[nn_idx] brute_time_sum += time.time() - begin_t print("Octree: build %.3f, knn %.3f, radius %.3f, brute %.3f" % (construction_time_sum * 1000 / iteration_num, knn_time_sum * 1000 / iteration_num, radius_time_sum * 1000 / iteration_num, brute_time_sum * 1000 / iteration_num)) print("KDTree --------------") construction_time_sum = 0 knn_time_sum = 0 radius_time_sum = 0 brute_time_sum = 0 for i in range(iteration_num): filename = os.path.join(data_dir, cat[i]) point_cloud = read_velodyne_bin(filename) # build tree: begin_t = time.time() kd_tree = KDTree(point_cloud=point_cloud, leaf_size=leaf_size) construction_time_sum += time.time() - begin_t query = point_cloud[0, :] # kNN query: begin_t = time.time() knn_result_set = KNNResultSet(capacity=k) kd_tree.knn_search(query, knn_result_set) knn_time_sum += time.time() - begin_t # RNN query: begin_t = time.time() rnn_result_set = RadiusNNResultSet(radius=radius) kd_tree.rnn_search(query, rnn_result_set) radius_time_sum += time.time() - begin_t # brute force: begin_t = time.time() diff = np.linalg.norm(point_cloud - query, axis=1) nn_idx = np.argsort(diff) nn_dist = diff[nn_idx] brute_time_sum += time.time() - begin_t print("Kdtree: build %.3f, knn %.3f, radius %.3f, brute %.3f" % (construction_time_sum * 1000 / iteration_num, knn_time_sum * 1000 / iteration_num, radius_time_sum * 1000 / iteration_num, brute_time_sum * 1000 / iteration_num))
def __init__(self, dim): self._parents_map = {} self._kd = KDTree(dim)
def __init__(self, dim, capacity=100000): self._edges = collections.defaultdict(list) self._kd = KDTree(dim, capacity) self.start_id = None self.target_id = None
from kdfinder import KDFinder import numpy as np import matplotlib.pyplot as plt import helper as h from stopwatch import StopWatch from kdtree import BucketedKDTree, KDTree a = np.random.rand(5000, 2) sw = StopWatch() obt = BucketedKDTree(a, optimized=True) sw.reset('Build time for Optimized BKD') bt = BucketedKDTree(a) sw.reset('Build time for BKD') t = KDTree(a) sw.reset('Build time for regular KD') for value in a: if not obt.has(value): print 'Missing Value!!' sw.reset('Traversal time for Optimized BKD') for value in a: if not bt.has(value): print 'Missing Value!!' sw.reset('Traversal time for BKD') for value in a: if not t.has(value): print 'Missing Value!!' sw.reset('Traversal time for regular KD') # a = np.asarray([[8,3],[9,2],[10,1],[7,4],[6,5],[5,6],[4,7],[3,8],[2,9],[1,10]])
def test_init(self): tree = KDTree(1) assert tree.root is None assert tree.size == 0 assert tree.is_empty() is True
# super profesjonalny plik z testami from kdtree import KDTree from sys import maxsize # TODO dodać pomiar czasu ### TEST 1 ### points1 = [(20, 50), (30, 40), (35, 60), (50, 100), (60, 70), (10, 45), (10, 10), (45, 23), (7, 8), (1, 3), (18, 90), (80, 80)] region1 = [(40, 60), (90, 110)] answer1 = [(60, 70), (80, 80), (50, 100)] root1 = KDTree(points1, None) result1 = root1.search(region1[0], region1[1]) print(" ########### TEST 1 ########### ") if sorted(result1) == sorted(answer1): print("Correct!") else: print("INCORRECT") print("Correct answer: ", end="") print(answer1) print("Your answer: ", end="") print(result1) print() ### TEST 2 ### points2 = [(20, 50), (30, 40), (35, 60), (50, 100), (60, 70), (10, 45),
def setUp(self): self.tree1 = KDTree([[2, 3], [5, 4], [9, 6], [4, 7], [8, 1], [7, 2]], leaf_size=1) self.tree2 = KDTree([[2, 3], [5, 4], [9, 6], [4, 7], [8, 1], [7, 2]], leaf_size=2)