def compute_cylinder_neighborhood(environment_pc, target_pc, radius):
"""Find the indices of points within a cylindrical neighbourhood (using KD Tree) for a given point of a target
point cloud among the points from an environment point cloud.
:param environment_pc: environment point cloud
:param target_pc: point cloud that contains the points at which neighborhoods are to be calculated
:param radius: search radius for neighbors
:return: indices of neighboring points from the environment point cloud for each target point
the returned indices also contains the index of the target point.
"""
avg_points_cyl = (radius * radius * math.pi) * POINT_CLOUD_DIST
x = target_pc[point]['x']['data']
if len(environment_pc[point]['x']['data']) == 0:
for _ in x:
yield [[] for _ in x]
return
cyl_size = avg_points_cyl * np.size(x) * sys.getsizeof(int)
mem_size = virtual_memory().total
print("Cylinder size in Bytes: %s" % cyl_size)
print("Memory size in Bytes: %s" % mem_size)
if cyl_size > mem_size * MEMORY_THRESHOLD:
y = target_pc[point]['y']['data']
num_points = int(
math.floor(mem_size * MEMORY_THRESHOLD /
(avg_points_cyl * sys.getsizeof(int))))
print("Number of points: %d" % num_points)
env_tree = kd_tree.get_kdtree_for_pc(environment_pc)
for i in range(0, np.size(x), num_points):
box_points = np.column_stack(
(x[i:min(i + num_points, np.size(x))],
y[i:min(i + num_points, np.size(x))]))
target_box_tree = cKDTree(box_points,
compact_nodes=False,
balanced_tree=False)
yield target_box_tree.query_ball_tree(env_tree, radius)
else:
print("Start tree creation")
env_tree = kd_tree.get_kdtree_for_pc(environment_pc)
print("Done with env tree creation")
target_tree = kd_tree.get_kdtree_for_pc(target_pc)
print("Done with target tree creation")
yield target_tree.query_ball_tree(env_tree, radius)
def _contains(pc, polygon):
"""
Return indices of points in point cloud that are contained by a polygon, i.e., all points within the boundaries of
Polygon excluding the ones overlaping Polygon's boundaries.
:param pc: point cloud in
:param polygon: containing polygon
:return: point indices
"""
x = pc[point]['x']['data']
y = pc[point]['y']['data']
points_in = []
mbr = polygon.envelope
point_box = box(np.min(x), np.min(y), np.max(x), np.max(y))
if point_box.intersects(mbr):
(x_min, y_min, x_max, y_max) = mbr.bounds
rad = math.ceil(math.sqrt(math.pow(x_max - x_min, 2) +
math.pow(y_max - y_min, 2)) / 2)
p = [x_min + ((x_max - x_min) / 2), y_min + ((y_max - y_min) / 2)]
tree = kd_tree.get_kdtree_for_pc(pc)
indices = np.sort(tree.query_ball_point(x=p, r=rad))
point_id = 0
for i in indices:
if polygon.contains(Point(x[i], y[i])):
points_in.append(i)
point_id += 1
return points_in
def test_sphere_neighb_kd_tree(self):
""" Tests whether sphere neighborhood gives good result """
tree = kd_tree.get_kdtree_for_pc(self.pointcloud)
rad = 10.
point_index = 1000
p = [
self.pointcloud[keys.point]["x"]["data"][point_index],
self.pointcloud[keys.point]["y"]["data"][point_index]
]
indices = tree.query_ball_point(p, rad)
for i in indices:
q = [
self.pointcloud[keys.point]["x"]["data"][i],
self.pointcloud[keys.point]["y"]["data"][i]
]
self.assertTrue((p[0] - q[0])**2 + (p[1] - q[1])**2 < rad**2)
def test_kd_tree_cache(self):
""" Tests the caching mechanism """
firsttree = kd_tree.get_kdtree_for_pc(self.pointcloud)
secondtree = kd_tree.get_kdtree_for_pc(self.pointcloud)
self.assertEqual(firsttree, secondtree)