点云投影到拟合平面python

原理

已知一组点云，通过RANSAC算法计算该点云的拟合平面，获得平面参数，其中三维空间平面方程定义为：Ax + By + Cz + D = 0。
定义不在拟合平面的点p₀(x₀,y₀,z₀)，投影到拟合平面后为p_p(x_p,y_p,z_p)，垂直投影过程应满足投影方程：



公式图片参考：点云侠：Open3D——点云投影到拟合平面(python详细过程版)

代码

RANSAC平面拟合，在python的open3D库中调用，距离阈值设置0.2，最小点数设置10。

import open3d as o3d
import numpy as np

def planeProject(points):  # 点云投影到平面，输出投影后得点
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    plane_model, _ = pcd.segment_plane(distance_threshold=0.2,  # 三维空间平面方程(Ax + By + Cz + D = 0)
                                       ransac_n=10,
                                       num_iterations=100)
    # 1.获取平面系数
    A = plane_model[0]
    B = plane_model[1]
    C = plane_model[2]
    D = plane_model[3]
    # 2.构建投影函数
    Xcoff = np.array([B * B + C * C, -A * B, -A * C])  # 列表
    Ycoff = np.array([-B * A, A * A + C * C, -B * C])
    Zcoff = np.array([-A * C, -B * C, A * A + B * B])
    # 3.三维坐标执行投影
    xp = (np.dot(points, Xcoff) - A * D) / (A * A + B * B + C * C)  # 列表
    yp = (np.dot(points, Ycoff) - B * D) / (A * A + B * B + C * C)
    zp = (np.dot(points, Zcoff) - C * D) / (A * A + B * B + C * C)
    project_points = np.c_[xp, yp, zp]  # 投影后的三维坐标(是按行连接两个矩阵，就是把两矩阵左右相加)

    # 显示
    pcd_project = o3d.geometry.PointCloud()
    pcd_project.points = o3d.utility.Vector3dVector(project_points)
    o3d.visualization.draw_geometries([pcd_project], window_name="投影",
                                      height=480, width=600,
                                      mesh_show_back_face=0)
    return project_points


cloud=o3d.io.read_point_cloud('D:\python\pointCloud/rabbit.pcd')
planeProject(np.asarray(cloud.points))

结果

原始数据：

投影结果：