def construct_octree(self, open3d_point_cloud: open3d.geometry.PointCloud,
include_color: bool) -> torch.Tensor:
# In case the point cloud has no points, add a single point
# This is a workaround because I was not able to create an empty octree without getting a segfault
# TODO: Figure out a better way of making an empty octree (it does not occur if setup correctly, so probably not worth it)
if not open3d_point_cloud.has_points():
open3d_point_cloud.points.append([
(self._min_bound[0] + self._max_bound[0]) / 2,
(self._min_bound[1] + self._max_bound[1]) / 2,
(self._min_bound[2] + self._max_bound[2]) / 2
])
open3d_point_cloud.normals.append([0.0, 0.0, 0.0])
open3d_point_cloud.colors.append([0.0, 0.0, 0.0])
# Convert open3d point cloud into octree points
octree_points = conversions.open3d_point_cloud_to_octree_points(
open3d_point_cloud, include_color)
# Convert octree points into 1D Tensor (via ndarray)
# Note: Copy of points here is necessary as ndarray would otherwise be immutable
octree_points_ndarray = np.frombuffer(np.copy(octree_points.buffer()),
np.uint8)
octree_points_tensor = torch.from_numpy(octree_points_ndarray)
# Finally, create an octree from the points
return self._points_to_octree(octree_points_tensor)
def preprocess_point_cloud(
self, open3d_point_cloud: open3d.geometry.PointCloud,
camera_frame_id: str, robot_frame_id: str, min_bound: List[float],
max_bound: List[float], normals_radius: float,
normals_max_nn: int) -> open3d.geometry.PointCloud:
# Check if any points remain in the area after cropping
if not open3d_point_cloud.has_points():
print("Point cloud has no points")
return open3d_point_cloud
# Get transformation from camera to robot and use it to transform point
# cloud into robot's base coordinate frame
transform = self.lookup_transform_sync(target_frame=robot_frame_id,
source_frame=camera_frame_id)
transform_mat = conversions.transform_to_matrix(transform=transform)
open3d_point_cloud = open3d_point_cloud.transform(transform_mat)
# Crop point cloud to include only the workspace
open3d_point_cloud = open3d_point_cloud.crop(
bounding_box=open3d.geometry.AxisAlignedBoundingBox(
min_bound=min_bound, max_bound=max_bound))
# Check if any points remain in the area after cropping
if not open3d_point_cloud.has_points():
print(
"Point cloud has no points after cropping to the workspace volume"
)
return open3d_point_cloud
# Estimate normal vector for each cloud point and orient these towards the camera
open3d_point_cloud.estimate_normals(
search_param=open3d.geometry.KDTreeSearchParamHybrid(
radius=normals_radius, max_nn=normals_max_nn),
fast_normal_computation=True)
open3d_point_cloud.orient_normals_towards_camera_location(
camera_location=transform_mat[0:3, 3])
return open3d_point_cloud