点云处理工具open3d使用

对于点云的处理，传统的工具可能是PCL,9.3k的github stars，纯c++的接口。但是随着open3d的问世，大家可以使用python直接对点云进行处理，操作简单，深受大家的喜欢，目前已经10.3k stars，已经超越了PCL，成为了最受欢迎的工具。这篇文章记录一下我使用open3d的代码，供大家参考。

官网：

isl-org/Open3D: Open3D: A Modern Library for 3D Data Processing (github.com)

安装：

pip install open3d 

官方文档：Open3D 0.18.0 documentation

主要内容：点云的读取和保存，rgb+depth的读取和转点云，点云的下采样，法向量估计，numpy和点云的转换，点云裁剪，可视化，分割平面算法，关键点提取，icp配准，聚类算法，边缘点提取，基于法向量的下采样，ppf配准等。

1.读取点云
model_pcd = o3d.io.read_point_cloud(path)  # 单位是米，值在[0,1]之间
model_points_array = np.array(model_pcd.points)

2.读dep，dep 2 pcd
pth = open(args.scene, 'rb')  # 我们的场景不是ply，是depth，把深度图转化为带法向量的点云、
depth_array = np.array(pickle.load(pth))
depth = o3d.geometry.Image(depth_array)
intrinsic = o3d.camera.PinholeCameraIntrinsic(width=depth_array.shape[1], height=depth_array.shape[0],
                                              fx=616.58, fy=616.778, cx=323.103,cy=238.464)
depth_pcd = o3d.geometry.PointCloud.create_from_depth_image(depth, intrinsic, depth_scale=1000, depth_trunc=1000.0) # depth_scale=1000后单位也是米
# depth_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
color0 = [0,0,1]
depth_pcd.paint_uniform_color(color0)

3.对点云体素下采样 &&估计法向量

up_points = model_points_array * 1000 # 单位 毫米
model_pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(up_points))

model_pcd.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=100, max_nn=30))  # 调节参数半径和邻居数量，根据单位
model_down_pcd = model_pcd.voxel_down_sample(voxel_size=8)  # 需要手动设置参数，保证下采样后的点云比较好的而保留了原始特征
downpcd_farthest = pcd.farthest_point_down_sample(500)

4.numpy转03d 点云
transformed_model_pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(transformed_model))
pcd_ary =  np.array(transformed_model_pcd.points)

5.点云裁剪
cropped_pcd = depth_pcd.crop(o3d.geometry.AxisAlignedBoundingBox(min_bound=np.min(transformed_bbox, axis=0),
                                                           max_bound=np.max(transformed_bbox, axis=0)))
                                                           
6.可视化                                                          
o3d.visualization.draw_geometries([transformed_model_pcd, cropped_pcd])

7.执行平面分割
plane_model, inliers = downpcd_farthest.segment_plane(distance_threshold=0.01, ransac_n=3, num_iterations=1000)
# 获取平面的参数
[a, b, c, d] = plane_model
# 根据平面参数筛选点云
obj_pcd = downpcd_farthest.select_by_index(inliers, invert=True)
boundarys, mask = obj_pcd.compute_boundary_points(0.1)  # 中间值最好，输入点云越少越好

8.iss关键点提取
keypoints = o3d.geometry.keypoint.compute_iss_keypoints(pcd)

9.icp
source = model_down_pcd
target = cropped_down_pcd
trans_init = np.vstack((np.hstack((r, t.reshape(-1, 1))), np.array([0, 0, 0, 1])))
threshold = 0.002  #  肥皂+笔：0.002
# point-to-plane ICP
reg_p2p = o3d.pipelines.registration.registration_icp(
    source, target, threshold, trans_init,
    #  TransformationEstimationPointToPlane,TransformationEstimationPointToPoint(),TransformationEstimationForGeneralizedICP,TransformationEstimationForColoredICP
    o3d.pipelines.registration.TransformationEstimationPointToPlane(),
    o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=5000)) 

10.聚类
# 聚类, 保留点数最多的哪个类作为目标
with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
    # eps表示聚类的领域距离，min_points表示聚类的最小点数。该函数返回一个label，其中label为-1表示为噪声。0应该时背景, 结果和半径有关
    labels = np.array(scene_cropped_down_pcd.cluster_dbscan(eps=0.01, min_points=20, print_progress=True))
points_array = np.array(scene_cropped_down_pcd.points)
points_array[labels < 0] = 0
cls_id_list = np.unique(labels[labels >= 0])
cls_pcd_num = {}
for i, cls_id in enumerate(cls_id_list):   # i 从0开始
    cls_i_pcd = points_array[labels == cls_id ]
    cls_pcd_num[cls_id] = len(cls_i_pcd)
cls_id = max(cls_pcd_num,key=cls_pcd_num.get)
aim_pcd = points_array[labels == cls_id]

11.边缘点提取
def extract_scene_edge_points(model_pcd):
    pcd = o3d.t.geometry.PointCloud.from_legacy(model_pcd, o3c.float64)
    downpcd_farthest = pcd.farthest_point_down_sample(5000)
    downpcd_farthest.estimate_normals(max_nn=30, radius=100)
    # 执行平面分割
    plane_model, inliers = downpcd_farthest.segment_plane(distance_threshold=0.01, ransac_n=3, num_iterations=1000)
    # 获取平面的参数
    [a, b, c, d] = plane_model
    # 根据平面参数筛选点云
    obj_pcd = downpcd_farthest.select_by_index(inliers, invert=True)
    # o3d.visualization.draw_geometries([obj_pcd.to_legacy()])
    boundarys, mask = obj_pcd.compute_boundary_points(0.1)  # 中间值最好，输入点云越少越好
    # TODO: not good to get size of points.
    # print(f"Detect {boundarys.point.positions.shape[0]} bnoundary points from {pcd.point.positions.shape[0]} points.")
    # print("检测后的点云数量", len(boundarys.point.positions))
    # boundarys = boundarys.paint_uniform_color([1.0, 0.0, 0.0])
    # o3d.visualization.draw_geometries([boundarys.to_legacy()])
    # 把6个面的轮廓点云拼接在一起返回一个边缘特征的模型然后计算ppf
    boundarys = boundarys.to_legacy()

    o3d.visualization.draw_geometries([boundarys])
    return boundarys
    
    
def pcd_normals_downsample(points, normals, radius, threshold):

    kdtree = KDTree(points)
    downsampled_points = []
    downsampled_normals = []

    for i in range(len(points)):
        query_result = kdtree.query_ball_point(points[i], radius)
        if len(query_result) > 1:
            neighbor_normals = normals[query_result[1:]]  # 去除自身点
            normal_diff = np.linalg.norm(normals[i] - neighbor_normals, axis=1)
            if np.max(normal_diff) > threshold:
                downsampled_points.append(points[i])
                downsampled_normals.append(normals[i])

    down_pcd = o3d.geometry.PointCloud()
    down_pcd.points = o3d.utility.Vector3dVector(downsampled_points)
    down_pcd.normals = o3d.utility.Vector3dVector(downsampled_normals)
    print(len(np.array(down_pcd.points)))
    if len(np.array(down_pcd.points)) > 50000:
        down_pcd = down_pcd.farthest_point_down_sample(50000)

    return down_pcd
    
    
def paxini_ppf_icp(model, scene, visual=False):
    "model,scene是ndarray，n*6(xyz,nxnynz)"
    N = 1
    print('-----------Training-----------')
    # 第一个参数：0.025：相对采样步长， 第二个：0.05：表示相对距离步长。这个参数用于决定匹配点对时的距离步长。设置为0.05且模型直径为1m（1000mm），则从对象表面采样的点将相距约50 mm
    detector = cv.ppf_match_3d_PPF3DDetector(0.0025, 0.005)
    detector.trainModel(model)
    print('-----------Matching-----------')
    results = detector.match(scene, 1.0 / 40.0, 0.05)                   # 参数1.0/40.0是匹配阈值，用于控制匹配的松紧程度。参数0.05是ICP阈值，用于滤除不可靠的匹配结果。
    print('-----------ICP-----------')
    icp = cv.ppf_match_3d_ICP(100)                                      # icp 100次
    _, results = icp.registerModelToScene(model, scene, results[:N])    # 这里结果取了前N个
    result = results[0]  # 第一个是vote点最多的pose
    print("-- Pose to Model Index %d: NumVotes = %d, Residual = %f\n%s\n" % (
        result.modelIndex, result.numVotes, result.residual, result.pose))
    # 可视化
    if visual == True:
        trans_model = np.dot(model[:, :3], result.pose[:3, :3].T) + result.pose[:3, 3]
        trans_model = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(trans_model))
        scene_pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(scene[:, :3]))
        trans_model.paint_uniform_color([1, 0, 0])
        scene_pcd.paint_uniform_color([0, 0, 1])
        o3d.visualization.draw_geometries([trans_model, scene_pcd])
    return result