pclpy 最小二乘法拟合平面

一、算法原理

平面方程的一般表达式为：
$$Ax+By+Cz+D=0(C\ne 0)$$
即：
$$Z=-\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C}$$
记：
$$a_0=-\frac{A}{C},a_1=-\frac{B}{C},a_2=-\frac{D}{C}$$
所以：
$$Z=a_{0}x+a_{1}y+a_2$$
要用该 n 个点拟合平面方程，即使：
$$\sum _{n=1}^N(a_{0}x+a_{1}y+a_2{)}^2->min$$
要使S最小，对a,b,c求偏导

改写成矩阵的形式为：

二、代码

from pclpy import pcl
import numpy as npdef CloudShow(cloud1, cloud2):"""Args:在一个窗口可视化多个点云cloud1: 点云数据1cloud2: 点云数据2"""viewer = pcl.visualization.PCLVisualizer("viewer")  # 建立可刷窗口对象 窗口名 viewerv0 = 1  # 设置标签名（0, 1标记第一个窗口）viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v0)  # 创建一个可视化的窗口viewer.setBackgroundColor(0.0, 0.0, 0.0, v0)  # 设置窗口背景为黑色single_color = pcl.visualization.PointCloudColorHandlerCustom.PointXYZ(cloud1, 255.0, 0, 0.0)  # 将点云设置为红色viewer.addPointCloud(cloud1,  # 要添加到窗口的点云数据。single_color,  # 指定点云的颜色"sample cloud1",  # 添加的点云命名v0)  # 点云添加到的视图v1 = 2  # 设置标签名（2代表第二个窗口）viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v1)  # 创建一个可视化的窗口viewer.setBackgroundColor(255.0, 255.0, 255.0, v1)  # 设置窗口背景为白色single_color = pcl.visualization.PointCloudColorHandlerCustom.PointXYZ(cloud2, 0.0, 255.0, 0.0)  # 将点云设置为绿色viewer.addPointCloud(cloud2,  # 要添加到窗口的点云数据。single_color,  # 指定点云的颜色"sample cloud2",  # 添加的点云命名v1)  # 点云添加到的视图# 设置点云窗口（可移除对点云可视化没有影响）viewer.setPointCloudRenderingProperties(0,  # 设置点云点的大小1,  # 点云像素"sample cloud1",  # 识别特定点云v0)  # 在那个窗口可视化viewer.setPointCloudRenderingProperties(0,  # 设置点云点的大小1,  # 点云像素"sample cloud2",  # 识别特定点云v1)  # 在那个窗口可视化# viewer.addCoordinateSystem(1.0)  # 设置坐标轴 坐标轴的长度为1.0# 窗口建立while not viewer.wasStopped():viewer.spinOnce(10)def plane(cloud, normal_vector):coeffs = pcl.ModelCoefficients()  # 创建了一个模型系数对象coeffs.values.append(normal_vector[0])  # a = 0.0coeffs.values.append(normal_vector[1])  # b = 0.0coeffs.values.append(normal_vector[2])  # c = 1.0coeffs.values.append(normal_vector[3])  # d = 0.0# 创建滤波器proj = pcl.filters.ProjectInliers.PointXYZ()  # 过滤器对象 proj，用于将点云投影到一个模型上。proj.setModelType(0)  # 模型类型被设为 0，代表使用平面模型。proj.setInputCloud(cloud)  # 将cloud点云数据进行处理proj.setModelCoefficients(coeffs)  # 处理参数coeffscloud_projected = pcl.PointCloud.PointXYZ()  # 建立保存点云proj.filter(cloud_projected)  # 将投影结果保存return cloud_projectedif __name__ == '__main__':cloud1 = pcl.PointCloud.PointXYZ()reader = pcl.io.PCDReader()  # 设置读取对象reader.read('res/bunny.pcd', cloud1)  # 读取点云保存在cloud中# 调用函数，生成离散点x, y, z = cloud1.x, cloud1.y, cloud1.zN =  cloud1.size()# ------------------------构建系数矩阵-----------------------------A = np.array([[sum(x ** 2), sum(x * y), sum(x)],[sum(x * y), sum(y ** 2), sum(y)],[sum(x), sum(y), N]])B = np.array([[sum(x * z), sum(y * z), sum(z)]])# 求解X = np.linalg.solve(A, B.T)print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f' % (X[0], X[1], X[2]))a, b, c, d = X[0], X[1], -1, X[2]plane_cloud = plane(cloud1, [a, b, c, d])  # 获得投影后的点云数据# ------------------ 可视化点云 -----------------CloudShow(cloud1, plane_cloud) 

三、结果

1.左边原点云、右边最小二乘法拟合平面后点云投影

四、相关数据

最小二乘法拟合参考链接：最小二乘拟合平面(python/C++版) - 知乎 (zhihu.com)