Open3D

创建/编辑点云,与Numpy数据类型的转换

  • 创建点云
# create point cloud from numpy array
pcd = o3d.geometry.PointCloud()
np_cloud = np.zeros((height*width,3))
np_color = np.zeros((height*width,3))
pcd.points = o3d.utility.Vector3dVector(np_cloud)
pcd.colors = o3d.utility.Vector3dVector(np_color)
  • 将点云转换为Numpy矩阵
# load point cloud to numpy array
xyz_load = np.asarray(pcd.points)

xyz_load为一个的矩阵

读写点云数据

o3d.io.write_point_cloud("./your_point_cloud.pcd", pcd)
source = o3d.io.read_point_cloud("./your_point_cloud.pcd")

可视化

  • 注意点云数据必须放入队列中
o3d.visualization.draw_geometries([pcd])

视角的优化

  • 在图形界面中转动/缩放到一个理想视角之后,按ctrl+c将在剪切板中生成一个当前视角配置的json,可以通过ctrl+v在图形界面中回到这一视角,或者粘贴到文本编辑器中
  • 可使用参数
    o3d.visualization.draw_geometries([pcd], 
                                      zoom=0.7, front=[-0.85, 0.5, 0.12], 
                                      lookat=[0.67,0.22,0], up=[0,0,1])
    

其它参数

  • left=1680设置显示窗口的位置偏移

显示坐标系

  • 声明坐标系,然后将坐标系传入draw_geometries
    axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.1, origin=[0, 0, 0])
    o3d.visualization.draw_geometries([pcd, axis_pcd])

绘制外框

  • OrientedBoundingBox(无法设置线框的粗细)
center = np.array([0, 0, 0])
r = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
size = np.array([10, 10, 10])
sample_3d = o3d.geometry.OrientedBoundingBox(center, r, size)
## set up color
sample_3d.color = [1,0,0]
 
o3d.visualization.draw_geometries([sample_3d],
                                  zoom=0.7,
                                  front=[0.5439, -0.2333, -0.8060],
                                  lookat=[2.4615, 2.1331, 1.338],
                                  up=[-0.1781, -0.9708, 0.1608])
  • 替代方案:
cylinders = []
r = 0.002
## z axis
for i in range(4):
    cylinders.append(o3d.geometry.TriangleMesh.create_cylinder(radius=r, height=x_max-x_min))
for i in range(4):
    cylinders.append(o3d.geometry.TriangleMesh.create_cylinder(radius=r, height=y_max-y_min))
for i in range(4):
    cylinders.append(o3d.geometry.TriangleMesh.create_cylinder(radius=r, height=z_max-z_min))
 
## along x-axis
x_pos = (x_max+x_min)/2
cylinders[0].transform(np.array([[0, 0, -1, x_pos],[0, 1, 0, y_min],[1, 0, 0, z_min],[0, 0, 0, 1]]))
cylinders[1].transform(np.array([[0, 0, -1, x_pos],[0, 1, 0, y_min],[1, 0, 0, z_max],[0, 0, 0, 1]]))
cylinders[2].transform(np.array([[0, 0, -1, x_pos],[0, 1, 0, y_max],[1, 0, 0, z_min],[0, 0, 0, 1]]))
cylinders[3].transform(np.array([[0, 0, -1, x_pos],[0, 1, 0, y_max],[1, 0, 0, z_max],[0, 0, 0, 1]]))
 
## along y-axis
y_pos = (y_max+y_min)/2
cylinders[4].transform(np.array([[1, 0, 0, x_min],[0, 0, 1, y_pos],[0, -1, 0, z_min],[0, 0, 0, 1]]))
cylinders[5].transform(np.array([[1, 0, 0, x_min],[0, 0, 1, y_pos],[0, -1, 0, z_max],[0, 0, 0, 1]]))
cylinders[6].transform(np.array([[1, 0, 0, x_max],[0, 0, 1, y_pos],[0, -1, 0, z_min],[0, 0, 0, 1]]))
cylinders[7].transform(np.array([[1, 0, 0, x_max],[0, 0, 1, y_pos],[0, -1, 0, z_max],[0, 0, 0, 1]]))
 
## along z-axis
z_pos = (z_max+z_min)/2
cylinders[8].transform(np.array([[1, 0, 0, x_min],[0, 1, 0, y_min],[0, 0, 1, z_pos],[0, 0, 0, 1]]))
cylinders[9].transform(np.array([[1, 0, 0, x_min],[0, 1, 0, y_max],[0, 0, 1, z_pos],[0, 0, 0, 1]]))
cylinders[10].transform(np.array([[1, 0, 0, x_max],[0, 1, 0, y_min],[0, 0, 1, z_pos],[0, 0, 0, 1]]))
cylinders[11].transform(np.array([[1, 0, 0, x_max],[0, 1, 0, y_max],[0, 0, 1, z_pos],[0, 0, 0, 1]]))
 
for i in cylinders:
    i.paint_uniform_color([0,0,0])
 
o3d.visualization.draw_geometries([pcd]+cylinders, 
                              zoom=0.7, front=[-0.85, 0.5, 0.12], 
                              lookat=[0.67,0.22,0], up=[0,0,1])

与ROS消息的数据类型转换

参考:open3d_ros_pointcloud_conversion

np_cloud = np.zeros((height*width,3))
# fill in your data here
FIELDS_XYZ = [PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
              PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
              PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),]
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "camera_depth_frame"
fields = FIELDS_XYZ
# PointCloud2 data
pc2_data = pc2.create_cloud(header, fields, np.asarray(np_cloud))

ICP点云配准

source = o3d.io.read_point_cloud("./data/cloud_bin_0.pcd")
    target = o3d.io.read_point_cloud("./data/cloud_bin_1.pcd")
    threshold = 0.02
    trans_init = np.asarray(
                [[0.862, 0.011, -0.507,  0.5],
                [-0.139, 0.967, -0.215,  0.7],
                [0.487, 0.255,  0.835, -1.4],
                [0.0, 0.0, 0.0, 1.0]])
evaluation = o3d.pipelines.registration.evaluate_registration(source, target,
            threshold, trans_init)
 
print("Apply point-to-point ICP")
 reg_p2p = o3d.pipelines.registration.registration_icp(source, target, threshold, trans_init,
            o3d.pipelines.registration.TransformationEstimationPointToPoint())
print("Apply point-to-plane ICP")
 reg_p2l = o3d.pipelines.registration.registration_icp(source, target, threshold, trans_init,
            o3d.pipelines.registration.TransformationEstimationPointToPlane())

DBSCAN

with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
    labels = np.array(pcd.cluster_dbscan(eps=self.eps, min_points=self.min_points, print_progress=False))
 
max_label = labels.max()
 
print(f"point cloud has {max_label + 1} clusters")
colors = plt.get_cmap("tab20")(labels / (max_label if max_label > 0 else 1))
colors[labels < 0] = 0
print(colors.shape)
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
o3d.visualization.draw_geometries([pcd],
                                  zoom=0.7, front=[-0.85, 0.5, 0.12],
                                  lookat=[0.67,0.22,0], up=[0,0,1])

Debug

  • 程序长时间没有响应,按ctrl+c后可以继续执行: open3d.utility.Vector3dVector()针对np.dtype=float64进行的优化,尝试进行数据类型转换 open3d.utility.Vector3dVector(np.array(data, dtype=np.float64))