Open3d: 点云平面拟合

因为项目需要分析点云数据， 此文总结其中拟合平面经验。

加载点云

import open3d as o3d
plypath = "/xxx/xxx.ply"
pcd = o3d.io.read_point_cloud(plypath)
o3d.visualization.draw_geometries([pcd])

效果如下:

平面拟合

plane_model, inliers = current_pcd.segment_plane(distance_threshold=0.3, ransac_n=5, num_iterations=1000)

其中三个参数：

• distance_threshold: 用于定义点到平面的最大距离阈值，单位为米。如果点到平面的距离小于此阈值，则认为该点属于平面。默认值为0.01
• ransac_n: 用于RANSAC算法的迭代次数。RANSAC是一种随机采样一致性算法，用于估计平面模型参数。默认值为3
• num_iterations: 在RANSAC算法中，每次迭代时使用的随机样本点的数量。默认值为10000

返回值：

• inliers: 拟合得到的点云索引
• plane_model: 平面模型\(Ax + By + Cz + D = 0\)，所以该返回值即为(A,B,C,D)元组。 (A,B,C)平面法向量， D为平面上一点到原点的有向距离。
  如已知法向量(0,0,2), D=-1, 则其平面如下图所示
  
  可见， 平面离原点的距离为0.5， 因为其法向量非单位向量， 对法向量除以2， D除以2，则得到平面到原点的有向距离。

拟合多个平面

上述点云中， 明显存在5个平面， 分别对应4面墙与地板。 而调用segment_plane仅仅得到一个平面的点云。想要达到每个平面拟合得到， 则需要分为几步:

1. 拟合一个平面
2. 将该平面在原始点云中删除， 得到新的点云b
3. 对b进行上述1、2步骤， 直到无法拟合处平面为止

代码示例:

def plane_detection(pcd, tolerance = 50):

    current_pcd = pcd
    planes = []
    while len(current_pcd.points) > tolerance:
        # 1. 拟合点云
        plane_model, inliers = current_pcd.segment_plane(distance_threshold=0.3, ransac_n=5, num_iterations=1000)
        # 如果不构成平面的必要条件， 则退出
        if len(inliers) < tolerance:
            break

        inlier_indices = np.asarray(inliers)
        plane_pcd = current_pcd.select_by_index(inlier_indices)
        # 2. 删除已拟合的平面，得到新的点云
        current_pcd = current_pcd.select_by_index(inlier_indices, invert=True)

        planes.append(plane_pcd)
    
    return planes，current_pcd

绘制平面法向量

已知平面模型plane_model， 求法向量两点代码示例如下：

# 生成法向量线段的端点坐标
normal_vector = plane_model[:3]
# 求平面上的一点
point_in_plane = -normal_vector * plane_model[3] / np.linalg.norm(normal_vector)**2 

# 沿着法向量方向上的另一点
endpoint = point_in_plane + normal_vector * 2 

# 创建法向量线段
line = o3d.geometry.LineSet()
line.points = o3d.utility.Vector3dVector([point_in_plane, endpoint])
line.lines = o3d.utility.Vector2iVector([[0, 1]])

绘制坐标轴

coordinate = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5.0, origin=[0,0,0])

最终效果

完整实例

import open3d as o3d
import numpy as np
import random


def plane_detection(pcd, tolerance = 50):

    current_pcd = pcd
    planes = []
    while len(current_pcd.points) > tolerance:
        plane_model, inliers = current_pcd.segment_plane(distance_threshold=0.3, ransac_n=5, num_iterations=1000)
        if len(inliers) < tolerance:
            break
        inlier_indices = np.asarray(inliers)
        inlier_cloud = current_pcd.select_by_index(inlier_indices)
        current_pcd = current_pcd.select_by_index(inlier_indices, invert=True)
        
        normal_vector = plane_model[:3]
        point_in_plane = -normal_vector * plane_model[3] / np.linalg.norm(normal_vector)**2 
        endpoint = point_in_plane + normal_vector * 2 

        line = o3d.geometry.LineSet()
        line.points = o3d.utility.Vector3dVector([point_in_plane, endpoint])
        line.lines = o3d.utility.Vector2iVector([[0, 1]])

        planes.append(line)
        planes.append(inlier_cloud)
    
    return current_pcd, planes
    


def main(plypath):
    pcd = o3d.io.read_point_cloud(plypath)

    remain_pcd, planes = plane_detection(pcd)
    for plane in planes:
        plane.paint_uniform_color(np.random.rand(3))
    
    mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5.0, origin=[0,0,0])
    # 可视化结果
    o3d.visualization.draw_geometries([ remain_pcd, *planes,mesh_frame])
    
    
main(r"xxx.ply")