def __insert_node(self, root, dim, parent, data_point): # TODO implement recursive insertion
if root is None:
pos = parent.bounding_box.get_centroid()
x_offset = dim[0] / 2.0
y_offset = dim[1] / 2.0
z_offset = dim[2] / 2.0
branch = self.__find_branch(parent, data_point)
if branch == 0:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] - x_offset, pos[1] - y_offset, pos[2] - z_offset]), x_offset, y_offset, z_offset)
if branch == 1:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] - x_offset, pos[1] - y_offset, pos[2] + z_offset]), x_offset, y_offset, z_offset)
if branch == 2:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] - x_offset, pos[1] + y_offset, pos[2] - z_offset]), x_offset, y_offset, z_offset)
if branch == 3:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] - x_offset, pos[1] + y_offset, pos[2] + z_offset]), x_offset, y_offset, z_offset)
if branch == 4:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] + x_offset, pos[1] - y_offset, pos[2] - z_offset]), x_offset, y_offset, z_offset)
if branch == 5:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] + x_offset, pos[1] - y_offset, pos[2] + z_offset]), x_offset, y_offset, z_offset)
if branch == 6:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] + x_offset, pos[1] + y_offset, pos[2] - z_offset]), x_offset, y_offset, z_offset)
if branch == 7:
new_box = AxisAlignedBox3D.init_box_center(
np.asarray([pos[0] + x_offset, pos[1] + y_offset, pos[2] + z_offset]), x_offset, y_offset, z_offset)
return OctreeNode(new_box, parent.depth + 1, [data_point])
elif not root.isLeafNode and (np.any(root.bounding_box.get_centroid() != data_point)
or root.bounding_box.get_centroid() != data_point):
branch = self.__find_branch(root, data_point)
new_dim = root.bounding_box.get_dimensions() / 2.0
root.branches[branch] = self.__insert_node(root.branches[branch], new_dim, root, data_point)
elif root.isLeafNode:
if len(root.data_points) < self.MAX_OBJECTS_PER_CUBE:
root.data_points.append(data_point)
# print(len(root.data_points))
# try:
# root.data_points.append(data_point)
# except AttributeError:
# print(root.data_points)
# print(type(root.data_points))
# exit(1)
else:
root.data_points.append(data_point)
objects = copy.deepcopy(root.data_points) # TODO do we need deepcopy here?
root.data_points = []
root.isLeafNode = False
new_dim = root.bounding_box.get_dimensions() / 2.0
for k in objects:
branch = self.__find_branch(root, k)
root.branches[branch] = self.__insert_node(root.branches[branch], new_dim, root, k)
return root
def naive_slice(data, slice_box=AxisAlignedBox3D()):
points = []
lower_corner = slice_box.min_corner()
upper_corner = slice_box.max_corner()
for datum in tqdm(data, total=len(data), desc="Slicing"):
if lower_corner[0] <= datum[0] <= upper_corner[0] and lower_corner[1] <= datum[1]\
<= upper_corner[1] and lower_corner[2] <= datum[2] <= upper_corner[2]:
points.append(np.array(datum))
return points
def __init__(self,
bounding_box=AxisAlignedBox3D(),
depth=0,
data_points=[]):
"""
branch: 0 1 2 3 4 5 6 7
x: - - - - + + + +
y: - - + + - - + +
z: - + - + - + - +
"""
self.bounding_box = bounding_box
self.depth = depth
self.data_points = data_points
#self.bounding_box_center = bounding_box.get_centroid()
#self.bounding_box_dimensions = bounding_box.get_dimensions()
self.isLeafNode = True
self.branches = [None] * 8
self.lower_corner = bounding_box.min_corner()
self.upper_corner = bounding_box.max_corner()
def naive_slice_from_las(filename, slice_box=AxisAlignedBox3D()):
points = []
with File(filename, mode='r') as in_file:
scales = in_file.header.scale
offsets = in_file.header.offset
x_s, y_s, z_s = scales[0], scales[1], scales[2]
x_o, y_o, z_o = offsets[0], offsets[1], offsets[2]
lower_corner = slice_box.min_corner()
upper_corner = slice_box.max_corner()
for datum in tqdm(in_file.points, total=len(in_file.points), desc="Slicing"):
# print([datum[0][0], datum[0][1], datum[0][2]])
s0 = scale(datum[0][0], x_s, x_o)
s1 = scale(datum[0][1], y_s, y_o)
s2 = scale(datum[0][2], z_s, z_o)
if lower_corner[0] <= s0 <= upper_corner[0]\
and lower_corner[1] <= s1 <= upper_corner[1]\
and lower_corner[2] <= s2 <= upper_corner[2]:
points.append(np.array([s0, s1, s2]))
print("sliced %d points" % len(points))
return points
from laspy.file import File
import numpy as np
from Utils.AxisAlignedBox3D import AxisAlignedBox3D
from SliceFunctions.NaiveSliceFromLAS import naive_slice_from_las
from PlotUtils.VtkPointCloud import VtkPointCloud
input_file = "../MantecaDock/dock.las"
output_file = "../MantecaDock/fourPallets.las"
inFile = File(input_file, mode='r')
outFile = File(output_file, mode='w', header=inFile.header)
pointCloud = VtkPointCloud()
sliced = naive_slice_from_las(
input_file, AxisAlignedBox3D([204.24, -6.7, -1.9], [208.46, -3.24, 3.5]))
# sliced = [[1, 1, 1]]
print("Number of points sliced: %d" % len(sliced))
# for point in sliced:
# pointCloud.addPoint(point)
#
# outFile.write(point)
# pdb.set_trace()
allx = np.array([sliced[i][0] for i in range(len(sliced))])
ally = np.array([sliced[i][1] for i in range(len(sliced))])
allz = np.array([sliced[i][2] for i in range(len(sliced))])
outFile.x = allx
outFile.y = ally
outFile.z = allz
# print("Number of points: %d" % len(sliced))
# print('Plotting')
import numpy as np from Utils.AxisAlignedBox3D import AxisAlignedBox3D x = AxisAlignedBox3D(np.array([0, 0, 0], dtype=np.float32), np.array([1, 1, 1], dtype=np.float32)) y = AxisAlignedBox3D.init_box_center(np.array([.5, .5, .5], dtype=np.float32), .5, .5, .5) print(x.get_corners()) print(y.get_corners()) assert x.equals_box(y), "fail equality"
# pc2 = create_vtkpc_from_array(points2)
# to_plot.append(pc2)
# points2 = read_raw_las_data(input2)
# pc2 = create_vtkpc_from_array(points2)
# to_plot.append(pc2)
# points = read_raw_las_data(input1)
# warehouse = subsample_frac_from_las_data(input2, .1)
#
# # points = subsample_frac_from_las_data(input1, .01)
# pc = create_vtkpc_from_array(points+warehouse)
# to_plot.append(pc)
points = naive_slice_from_las(input1,
AxisAlignedBox3D([7, -13, -.5], [9, -11, 3]))
# points = subsample_frac_from_las_data(input1, .01)
points = ann_guided_filter(points, num_neighbors=50, filter_eps=.07)
# points = subsample_frac_from_las_data(input1, .1)
pc = create_vtkpc_from_array(points)
to_plot.append(pc)
# points2 = threshold_filter(points, threshold=.01)
# pc2 = create_vtkpc_from_array(points2)
# to_plot.append(pc2)
# points2 = read_raw_las_data(input2)
# pc2 = create_vtkpc_from_array(points2)
# to_plot.append(pc2)
# points2 = threshold_filter(points, threshold=.015)
def __init__(self, bounding_box=AxisAlignedBox3D(np.asarray([0., 0., 0.]), np.asarray([2048., 2048., 2048.]))):
self.MAX_OBJECTS_PER_CUBE = 1000
self.root = OctreeNode(bounding_box, 0, [])
def __on_keep_points_inside_box_click(self):
"""
A function that prompts for two points to define an AxisAlignedBox3D and all points INSIDE the box are kept
"""
prompt = QInputDialog.getInt(self, "Plot index to cull", "Index")
# note that prompt returns as ('int_inputted', bool) where bool represents if the prompt was taken
if prompt[1]:
try:
i = prompt[0]
w = self.widgets[
i] # only to throw the Index Error if invalid index given
except IndexError:
QMessageBox.about(
self, "Error",
"Index out of bounds exception, remember to zero index.")
return
else:
return
corner0 = QInputDialog.getText(self, "Bounding Point 1", "")
# regex from https://stackoverflow.com/questions/12929308/
# python-regular-expression-that-matches-floating-point-numbers/12929311
comp = re.compile(
'\([+-]?(\d+(\.\d*)?|\.\d+)([eE][+-]?\d+)?'
'(\s*,\s*[+-]?(\d+(\.\d*)?|\.\d+)([eE][+-]?\d+)?){2}\)')
if corner0[1]:
m = comp.match(corner0[0])
if m:
# print("Match found: ", m.group())
temp = m.group().replace("(", "")
temp = temp.replace(")", "")
temp = re.sub(r"\s+", "", temp).split(",")
corner_float0 = [float(i) for i in temp]
# print(corner_float0)
else:
print("Invalid point syntax")
return
else:
return
corner1 = QInputDialog.getText(self, "Bounding Point 2", "")
if corner1[1]:
m = comp.match(corner1[0])
if m:
# print("Match found: ", m.group())
temp = m.group().replace("(", "")
temp = temp.replace(")", "")
temp = re.sub(r"\s+", "", temp).split(",")
corner_float1 = [float(i) for i in temp]
# print(corner_float0)
else:
print("Invalid point syntax")
return
else:
return
# print(corner_float0)
# print(corner_float1)
bounding_box = AxisAlignedBox3D(corner_float0, corner_float1)
new_min = bounding_box.min_corner()
points_to_keep = []
point_array = self.plots[i].getPointsAsArray()
for point in tqdm(point_array,
total=len(point_array),
desc="Clearing Points"):
if bounding_box.contains_point(point):
points_to_keep.append(point[:])
del point_array
self.plots[i].clearPoints()
for point in tqdm(points_to_keep,
total=len(points_to_keep),
desc="Constructing PC"):
self.plots[i].addPoint(point)
del points_to_keep
self.__translate_helper(i, -new_min)
class TestObject:
def __init__(self, name, position):
self.name = name
self.position = position
@property
def __str__(self):
return u"name: {0} position: {1}".format(self.name, self.position)
NUM_TEST_OBJECTS = 20000
NUM_LOOKUPS = 20000
enclosure = AxisAlignedBox3D(np.asarray([0., 0., 0.]),
np.asarray([128., 128., 128.]))
center = enclosure.get_centroid()
testObjects = []
for x in range(NUM_TEST_OBJECTS):
name = "Node__" + str(x)
pos = np.array(center + np.random.uniform(0., 64., 3))
testObjects.append(TestObject(name, pos))
tree = Octree(enclosure)
Start = time.time()
for test in testObjects:
tree.insert_node(test.position)
End = time.time() - Start
print("%d node tree generated in %f seconds" % (NUM_TEST_OBJECTS, End))
input_file = "../MantecaRoom1/room1.las"
# out_file = "../MantecaDock/smallArea.las"
pointCloud = VtkPointCloud()
filteredPointCloud = VtkPointCloud()
with File(input_file, mode='r') as f:
input_header = f.header
print("reading %s" % input_file)
points = read_raw_las_data(input_file)
for point in tqdm(points, total=len(points), desc="Adding"):
pointCloud.addPoint(point)
filtered_points = naive_slice(
points, AxisAlignedBox3D([-70, -20, 1.94], [10, 90, 2.54]))
for filtered_point in tqdm(filtered_points,
total=len(filtered_points),
desc="Adding"):
filteredPointCloud.addPoint(filtered_point)
print("Original has %d points" % len(points))
print("Sliced has %d points" % len(filtered_points))
to_plot = [pointCloud, filteredPointCloud]
create_point_cloud_plot_qt(to_plot)
# # inFile = File(out_file, mode='r')
# # outFile = File(out_file, mode='w', header=inFile.header)
#
# pointCloud = VtkPointCloud()
from PlotUtils.VtkPointCloud import VtkPointCloud
input_file = "../MantecaRoom1/rack.las"
out_file = "../MantecaDock/smallArea.las"
pointCloud = VtkPointCloud()
filteredPointCloud = VtkPointCloud()
# inFile = File(out_file, mode='r')
# outFile = File(out_file, mode='w', header=inFile.header)
pointCloud = VtkPointCloud()
print("reading from %s" % input_file)
sliced = naive_slice_from_las(
input_file,
AxisAlignedBox3D([197.5000, -6.7000, -.2000], [209.5000, 1.0600, 8.6000]))
# pdb.set_trace()
# 1975000, 2095000, -67000, 10600, -2000, 86000 minx maxx miny maxy minz maxz
for point in tqdm(sliced, total=len(sliced), desc="Adding"):
pointCloud.addPoint(point)
print("Number of points: %d" % len(sliced))
print('Plotting')
# Renderer
renderer = vtk.vtkRenderer()
renderer.AddActor(pointCloud.vtkActor)
renderer.SetBackground(.2, .3, .4)
renderer.ResetCamera()
# Render Window
renderWindow = vtk.vtkRenderWindow()