pclpy 拉格朗日乘子法拟合平面

一、算法原理

1.算法步骤

对k一近邻点拟合平面，最小二乘法（平面过重心），拟定公式为
$$a(x-\frac{}{x})+b(y-\frac{}{y})+c(z-\frac{}{z})=0$$

1. 求重心
   $$\frac{}{x}=\frac{1}{N}\sum _{i=1}^{N}xi,\frac{}{y}=\frac{1}{N}\sum _{i=1}^{N}yi,\frac{}{z}=\frac{1}{N}\sum _{i=1}^{N}zi$$
2. 去重心化
   $$x=xi-\frac{}{x},y=yi-\frac{}{y},x=zi-\frac{}{z}$$
3. 拉格朗日乘子法求函数
   $$min(\sum _{i=1}^{N}i\ast \sum _{i=1}^{N}i)=\sum _{i=1}^N(ax+by+cz)\ast (ax+by+cz)$$
   4）求偏导

1.4.1 对a求偏导
$$2xi\sum _{i=1}^{N}axi+byi+czi$$
1.4.2 对b求偏导
$$2yi\sum _{i=1}^{N}axi+byi+czi$$
1.4.3 对c求偏导
$$2zi\sum _{i=1}^{N}axi+byi+czi$$
1.4.3 矩阵化

5. 求出最小特征向量

二、代码

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
    # 求重心
    x0 = np.mean(x)   # 计算平均值
    y0 = np.mean(y)   # 计算平均值
    z0 = np.mean(z)   # 计算平均值
    # 去重心
    x = x - x0
    y = y - y0
    z = z - z0
    # ------------------------构建系数矩阵-----------------------------
    A = np.array([[np.sum(x * x), np.sum(x * y), np.sum(x * z)],
                  [np.sum(x * y), np.sum(y * y), np.sum(y * z)],
                  [np.sum(x * z), np.sum(y * z), np.sum(z * z)]])
    [D, X] = np.linalg.eig(A)  # 计算矩阵的特征值和特征向量的函数

    # 找到最小特征值的索引
    min_eigenvalue_index = np.argmin(D)

    # 获取对应的特征向量
    min_eigenvector = X[:, min_eigenvalue_index]
    print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f' % (min_eigenvector[0], min_eigenvector[1], min_eigenvector[2]))

    # 平面参数
    a, b, c, d = min_eigenvector[0], min_eigenvector[1], min_eigenvector[2], -min_eigenvector[0]*x0 - min_eigenvector[1]*y0 - min_eigenvector[2]*z0
    plane_cloud = plane(cloud1, [a, b, c, d])  # 获得投影后的点云数据
    # ------------------ 可视化点云 -----------------

    CloudShow(cloud1, plane_cloud)  # 可视化点云

三、结果

1.左边原点云数据，右边将点云拉格朗日乘子法拟合平面投影在该平面

四、相关数据

open3d 将点云投影到平面-CSDN博客：open3d 将点云投影到平面-CSDN博客

pclpy 参数模型投影：pclpy 参数模型投影-CSDN博客

拉格朗日乘子法理论参考：最小二乘拟合平面(python/C++版) - 知乎 (zhihu.com)