def save_voxel(self, pos):
x, y, z = tools.find_min_max(self.cloud.to_list())
begin_x = tools.round_down_multiple_of(x[0], self.voxel_size)
end_x = tools.round_up_multiple_of(x[1], self.voxel_size)
begin_y = tools.round_down_multiple_of(y[0], self.voxel_size)
end_y = tools.round_up_multiple_of(y[1], self.voxel_size)
safe_begin_x = tools.round_down_multiple_of(
pos[0] - 3 * self.voxel_size, self.voxel_size)
safe_end_x = tools.round_up_multiple_of(pos[0] + 3 * self.voxel_size,
self.voxel_size)
safe_begin_y = tools.round_down_multiple_of(
pos[1] - 3 * self.voxel_size, self.voxel_size)
safe_end_y = tools.round_up_multiple_of(pos[1] + 3 * self.voxel_size,
self.voxel_size)
for i in range((end_x - begin_x) / self.voxel_size):
for j in range((end_y - begin_y) / self.voxel_size):
temp_x = begin_x + i * self.voxel_size
temp_y = begin_y + j * self.voxel_size
mid_x = temp_x - 0.5 * self.voxel_size
mid_y = temp_y - 0.5 * self.voxel_size
if not (mid_x > safe_begin_x and mid_x < safe_end_x
and mid_y > safe_begin_y and mid_y < safe_end_y):
cbf = self.cloud.make_cropbox()
cbf.set_Min(temp_x, temp_y, z[0], 1.0)
cbf.set_Max(temp_x + self.voxel_size,
temp_y + self.voxel_size, z[1], 1.0)
cloud_out = cbf.filter()
pcl.save(cloud_out,
"x" + str(temp_x) + "-y" + str(temp_y) + ".pcd")
cbf = self.cloud.make_cropbox()
cbf.set_Min(safe_begin_x, safe_begin_y, z[0], 1.0)
cbf.set_Max(safe_end_x, safe_end_y, z[1], 1.0)
cloud_out = cbf.filter()
self.cloud = cloud_out
relevant_ags = self.agManager.radius_search_points(
pos, 3 * self.voxel_size, 100)
self.agManager.drop_not_listed(relevant_ags)
def concat_depth_map(self, pos, projection, view, left_view):
points_list, vert, horz = self.camera.convert(pos, projection, view,
left_view)
x, y, z = tools.find_min_max(points_list)
p, r, y = view
lp, lr, ly = left_view
my_pts = self.cloud.to_list()
for point in my_pts:
#if in space and keeping points, don't remove, else remove
if self.in_view(pos, point, view, left_view, vert,
horz) and point not in points_list:
my_pts.remove(point)
#add new points
for point in points_list:
if point not in my_pts:
my_pts.append(point)
self.cloud = pcl.PointCloud(list(my_pts))
def load_voxel(self, pos):
x, y, z = tools.find_min_max(self.cloud.to_list())
safe_begin_x = tools.round_down_multiple_of(
pos[0] - 2 * self.voxel_size, self.voxel_size)
safe_end_x = tools.round_up_multiple_of(pos[0] + 2 * self.voxel_size,
self.voxel_size)
safe_begin_y = tools.round_down_multiple_of(
pos[1] - 2 * self.voxel_size, self.voxel_size)
safe_end_y = tools.round_up_multiple_of(pos[1] + 2 * self.voxel_size,
self.voxel_size)
for i in range((safe_end_x - safe_begin_x) / self.voxel_size):
for j in range((safe_end_y - safe_begin_y) / self.voxel_size):
temp_x = safe_begin_x + i * self.voxel_size
temp_y = safe_begin_y + j * self.voxel_size
try:
name = "x" + str(temp_x) + "-y" + str(temp_y) + ".pcd"
temp_c = pcl.load(name)
self.cloud = self.cloud + temp_c
except:
pass
def replace_points_list(self, points_list):
x, y, z, = tools.find_min_max(points_list)
self.replace(points_list, x, y, z)
def concat_literal_cloud(self, new_cloud):
new_pts = new_cloud.to_list()
x, y, z = tools.find_min_max(new_pts)
self.replace(new_cloud, x, y, z)
def get_min_max_3D(self):
x, y, z = tools.find_min_max(self.cloud.to_list())