<jupyter_start><jupyter_text>Basic Point Cloud Operations Make sure to pull the Open3D github repository in order to get access to all the examples below. https://github.com/isl-org/Open3D<jupyter_code>open3dpath="../../../Open3D/examples/test_data/"
print("Load a ply point cloud, print it, and render it")
pcd = o3d.io.read_point_cloud(open3dpath + "fragment.ply")
print(pcd)
print(np.asarray(pcd.points))
o3d.visualization.draw_geometries([pcd],
                                  zoom=0.3412,
                                  front=[0.4257, -0.2125, -0.8795],
                                  lookat=[2.6172, 2.0475, 1.532],
                                  up=[-0.0694, -0.9768, 0.2024])
print("Downsample the point cloud with a voxel of 0.05")
downpcd = pcd.voxel_down_sample(voxel_size=0.05)
o3d.visualization.draw_geometries([downpcd],
                                  zoom=0.3412,
                                  front=[0.4257, -0.2125, -0.8795],
                                  lookat=[2.6172, 2.0475, 1.532],
                                  up=[-0.0694, -0.9768, 0.2024])
print("Recompute the normal of the downsampled point cloud")
downpcd.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
o3d.visualization.draw_geometries([downpcd],
                                  zoom=0.3412,
                                  front=[0.4257, -0.2125, -0.8795],
                                  lookat=[2.6172, 2.0475, 1.532],
                                  up=[-0.0694, -0.9768, 0.2024],
                                  point_show_normal=True)
print("Load a polygon volume and use it to crop the original point cloud")
vol = o3d.visualization.read_selection_polygon_volume(
    open3dpath+"Crop/cropped.json")
chair = vol.crop_point_cloud(pcd)
o3d.visualization.draw_geometries([chair],
                                  zoom=0.7,
                                  front=[0.5439, -0.2333, -0.8060],
                                  lookat=[2.4615, 2.1331, 1.338],
                                  up=[-0.1781, -0.9708, 0.1608])
print("Paint chair")
chair.paint_uniform_color([1, 0.706, 0])
o3d.visualization.draw_geometries([chair],
                                  zoom=0.7,
                                  front=[0.5439, -0.2333, -0.8060],
                                  lookat=[2.4615, 2.1331, 1.338],
                                  up=[-0.1781, -0.9708, 0.1608])
# Load data
print("Select points a distance from a point cloud away")
pcd = o3d.io.read_point_cloud(open3dpath+"fragment.ply")
vol = o3d.visualization.read_selection_polygon_volume(
    open3dpath+"Crop/cropped.json")
chair = vol.crop_point_cloud(pcd)

dists = pcd.compute_point_cloud_distance(chair)
dists = np.asarray(dists)
ind = np.where(dists > 0.01)[0]
pcd_without_chair = pcd.select_by_index(ind)
o3d.visualization.draw_geometries([pcd_without_chair],
                                  zoom=0.3412,
                                  front=[0.4257, -0.2125, -0.8795],
                                  lookat=[2.6172, 2.0475, 1.532],
                                  up=[-0.0694, -0.9768, 0.2024])
print("Estimated bounding boxes of point cloud")
aabb = chair.get_axis_aligned_bounding_box()
aabb.color = (1, 0, 0)
obb = chair.get_oriented_bounding_box()
obb.color = (0, 1, 0)
o3d.visualization.draw_geometries([chair, aabb, obb],
                                  zoom=0.7,
                                  front=[0.5439, -0.2333, -0.8060],
                                  lookat=[2.4615, 2.1331, 1.338],
                                  up=[-0.1781, -0.9708, 0.1608])
print("Compute complex hull")
hull, _ = chair.compute_convex_hull()
hull_ls = o3d.geometry.LineSet.create_from_triangle_mesh(hull)
hull_ls.paint_uniform_color((1, 0, 0))
o3d.visualization.draw_geometries([chair, hull_ls],
                                  zoom=0.7,
                                  front=[0.5439, -0.2333, -0.8060],
                                  lookat=[2.4615, 2.1331, 1.338],
                                  up=[-0.1781, -0.9708, 0.1608])
print("Segment point cloud into individual objects")
import matplotlib.pyplot as plt

pcd = o3d.io.read_point_cloud(open3dpath+"fragment.ply")

with o3d.utility.VerbosityContextManager(
        o3d.utility.VerbosityLevel.Debug) as cm:
    labels = np.array(
        pcd.cluster_dbscan(eps=0.02, min_points=10, print_progress=True))

max_label = labels.max()
print("fpoint cloud has {max_label + 1} clusters")
colors = plt.get_cmap("tab20")(labels / (max_label if max_label > 0 else 1))
colors[labels < 0] = 0
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
o3d.visualization.draw_geometries([pcd],
                                  zoom=0.455,
                                  front=[-0.4999, -0.1659, -0.8499],
                                  lookat=[2.1813, 2.0619, 2.0999],
                                  up=[0.1204, -0.9852, 0.1215])
pcd = o3d.io.read_point_cloud(open3dpath+"fragment.ply")
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
                                         ransac_n=3,
                                         num_iterations=1000)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

inlier_cloud = pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([1.0, 0, 0])
outlier_cloud = pcd.select_by_index(inliers, invert=True)
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud],
                                  zoom=0.8,
                                  front=[-0.4999, -0.1659, -0.8499],
                                  lookat=[2.1813, 2.0619, 2.0999],
                                  up=[0.1204, -0.9852, 0.1215])

print("Convert mesh to a point cloud and estimate dimensions")
#pcd = o3dtut.get_armadillo_mesh().sample_points_poisson_disk(5000)
diameter = np.linalg.norm(
    np.asarray(pcd.get_max_bound()) - np.asarray(pcd.get_min_bound()))
print(diameter)
o3d.visualization.draw_geometries([pcd],
                                  zoom=0.3412,
                                  front=[0.4257, -0.2125, -0.8795],
                                  lookat=[2.6172, 2.0475, 1.532],
                                  up=[-0.0694, -0.9768, 0.2024])
print("Define parameters used for hidden_point_removal")
camera = [0, 0, diameter]
radius = diameter * 100

print("Get all points that are visible from given view point")
_, pt_map = pcd.hidden_point_removal(camera, radius)

print("Visualize result")
pcd = pcd.select_by_index(pt_map)
o3d.visualization.draw_geometries([pcd])<jupyter_output>Define parameters used for hidden_point_removal
Get all points that are visible from given view point
Visualize result<jupyter_text>RGBD Images<jupyter_code>print("Read Redwood dataset")
color_raw = o3d.io.read_image(open3dpath+"RGBD/color/00000.jpg")
depth_raw = o3d.io.read_image(open3dpath+"RGBD/depth/00000.png")
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_raw, depth_raw)
print(rgbd_image)
plt.subplot(1, 2, 1)
plt.title('Redwood grayscale image')
plt.imshow(rgbd_image.color)
plt.subplot(1, 2, 2)
plt.title('Redwood depth image')
plt.imshow(rgbd_image.depth)
plt.show()
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
    rgbd_image,
    o3d.camera.PinholeCameraIntrinsic(
        o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault))
# Flip it, otherwise the pointcloud will be upside down
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
o3d.visualization.draw_geometries([pcd])<jupyter_output><empty_output><jupyter_text>Webots<jupyter_code>from controller import Robot
from enum import Enum
import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt

# create the Robot instance.
robot = Robot()

# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())

rangefinder = robot.getDevice('range-finder')
rangefinder.enable(timestep)

camera = robot.getDevice('camera')
camera.enable(timestep)

for i in range(10):
    robot.step(timestep)
depth_1darray = np.frombuffer(rangefinder.getRangeImage(data_type="buffer"), dtype=np.float32)
depth=np.reshape(depth_1darray,(240,320))
depth=depth*1000.0

image_1darray = camera.getImage()
image = np.frombuffer(image_1darray, np.uint8).reshape((camera.getHeight(), camera.getWidth(), 4))


robot.step(timestep)
plt.subplot(1, 2, 1)
plt.title('Camera image')
plt.imshow(image)
plt.subplot(1, 2, 2)
plt.title('Depth image')
plt.imshow(depth)
plt.show()
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
        o3d.geometry.Image(image), 
        #o3d.geometry.Image(depth),
        o3d.geometry.Image(np.array(depth).astype('uint16')),
        convert_rgb_to_intensity=False,
        depth_scale=1000.0, depth_trunc=3.49)

pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
    rgbd_image,
       o3d.camera.PinholeCameraIntrinsic(320,240,320,240,160,120),
    project_valid_depth_only=True
)
# Flip it, otherwise the pointcloud will be upside down
#pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
o3d.visualization.draw_geometries([pcd])
np.asarray(pcd.points)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.13155033702978908, 'fraction_numerical': 0.08143074581430745, 'mean_word_length': 2.3475254730713244, 'pattern_counts': {'":': 0, '<': 13, '<?xml version=': 0, '>': 14, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 49, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '3', 'dir': 'jupyter-structured-clean-dedup', 'id': '346185', 'license': 'MIT', 'n_tokens_mistral': 3687, 'n_tokens_neox': 3230, 'n_words': 567, 'path': 'csci4302manipulation/lab2_grasping/open3d_test.ipynb', 'repo_name': 'Introduction-to-Autonomous-Robots/labs', 'size': '9198'}