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)
