pclpy 最小二乘法拟合平面
一、算法原理
平面方程的一般表达式为：
A x + B y + C z + D = 0 ( C ≠ 0 ) Ax + By + Cz + D = 0 \quad (C\neq0) Ax+By+Cz+D=0(C=0)
即：
Z = − A C x − B C y − D C Z = -\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C} Z=−CAx−CBy−CD
记：
a 0 = − A C , a 1 = − B C , a 2 = − D C a_0 = -\frac{A}{C}, a_1 =-\frac{B}{C}, a_2=-\frac{D}{C} a0=−CA,a1=−CB,a2=−CD
所以：
Z = a 0 x + a 1 y + a 2 Z = a_0x + a_1y + a_2 Z=a0x+a1y+a2
要用该 n 个点拟合平面方程，即使：
∑ n = 1 N ( a 0 x + a 1 y + a 2 ) 2 − > m i n \sum_{n=1}^{N}{(a_0x + a_1y + a_2)^2}->min n=1∑N(a0x+a1y+a2)2−>min
要使S最小，对a,b,c求偏导
改写成矩阵的形式为：
二、代码
python
from pclpy import pcl
import numpy as np
def CloudShow(cloud1, cloud2):
"""
Args:在一个窗口可视化多个点云
cloud1: 点云数据1
cloud2: 点云数据2
"""
viewer = pcl.visualization.PCLVisualizer("viewer") # 建立可刷窗口对象 窗口名 viewer
v0 = 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.0
coeffs.values.append(normal_vector[1]) # b = 0.0
coeffs.values.append(normal_vector[2]) # c = 1.0
coeffs.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) # 处理参数coeffs
cloud_projected = pcl.PointCloud.PointXYZ() # 建立保存点云
proj.filter(cloud_projected) # 将投影结果保存
return cloud_projected
if __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.z
N = 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)