open3d svd求平面方程

一、算法原理

拟合出的平面方程为：
$$ax+by+cz+d=0$$
约束条件：
$$a^2+b^2+c^2=1$$
可以得到平面参数a,b,c,d。此时，要使获得的拟合平面是最佳的，就是使得点到该平面的距离的平方和最小，即满足：
$$e=\sum _{i=1}^N{a}_i^2->min$$

$$\begin{gathered} d_i是点云数据中的任一点p_i{p}_i(x_i,y_i,z_i)到这个平面距离d_i=|a{x}_i+b{y}_i+c{z}_i+d|要使e->min，可以使用 \\ SVD矩阵分解得到 \end{gathered}$$

推导过程如下：
$$\begin{gathered} 所以点平均坐标(\frac{}{x},\frac{}{y},\frac{}{z}) \\ 带入到方程中a\frac{}{x}+b\frac{}{y}+c\frac{}{z}+d=0 \end{gathered}$$
则可得函数
$$a(x_i-\frac{}{x})+b(y_i-\frac{}{y})+c(z_i-\frac{}{z})=0$$

理想情况下所有点都在平面上；实际情况下有部分点在平面外，拟合的目的为平面距离所有点的距离之和尽量小，所以目标函数为：

所以， e的最小值就是矩阵 A 的最小特征值，对应的特征向量为平面参数a,b,c,d,利用质心可求得 d。

二、代码

import open3d as o3d
import numpy as np

def plane(pcd, normal_vector):
    """
    Args:将点云投影到平面
        pcd:  点云数据
        normal_vector:  方程法向量 mx + ny + sz + d = 0  传入[m , n, s, d]
    Returns: 投影后的点云数据
    """
    plane_seeds = []  # 保存投影后的点云数据
    # 获取平面系数
    m = normal_vector[0]
    n = normal_vector[1]
    s = normal_vector[2]
    d = normal_vector[3]
    # 将点云转换为数组
    points = np.asarray(pcd.points)
    for xyz in points:
        x, y, z = xyz
        """
        t = -(m*x + n*y + s*z + d) / (m*m + n*n + s*s)  # 计算参数方程参数
        """
        t = -(m * x + n * y + s * z + d) / (m * m + n * n + s * s)  # 计算参数方程参数
        """
        xi = m*t + x    # 计算x的投影
        yi = b*t + y    # 计算y的投影
        zi = s*t + z    # 计算z的投影
        """
        xi = m * t + x  # 计算x的投影
        yi = n * t + y  # 计算y的投影
        zi = s * t + z  # 计算z的投影
        plane_seeds.append([xi, yi, zi])  # 将投影后的点云添加到数组中

    plane_cloud = o3d.geometry.PointCloud()  # 使用numpy生成点云
    plane_cloud.points = o3d.utility.Vector3dVector(plane_seeds)  # points numpy数组

    return plane_cloud


if __name__ == '__main__':
    """ 拟合出的平面方程为： ax + by +cz + d = 0"""
    """约束条件 a^2 + b^2 + c^2 = 1"""
    pcd = o3d.io.read_point_cloud('res/bunny.pcd')
    points = np.asarray(pcd.points)  # 将点云文件转换为数组

    # 获得平均值
    avg_x = np.mean(points[:, 0])
    avg_y = np.mean(points[:, 1])
    avg_z = np.mean(points[:, 2])

    # 去重心化
    x = points[:, 0] - avg_x
    y = points[:, 1] - avg_y
    z = points[:, 2] - avg_z
    # 生成矩阵
    A = np.transpose(np.asarray([x, y, z]))   # 将数据矩阵化

    """AX = 0"""
    # 计算协方差矩阵
    cov_matrix = np.cov(A, rowvar=False)

    # 使用 SVD 分解求解协方差矩阵的特征值和特征向量
    U, S, Vt = np.linalg.svd(cov_matrix)

    # 特征值和特征向量
    eigenvalues = S
    eigenvectors = Vt.T  # 转置后得到正确的特征向量 一行代表一个特征向量 最小的特征向量在最后一行

    print("特征值：", eigenvalues)
    print("特征向量：", eigenvectors)

    """如果想要找到最小的特征向量，可以直接从特征向量矩阵中取最后一列（没有转置前），
    因为特征向量是按特征值从大到小排列的，所以最后一列就是对应最小特征值的特征向量。"""
    X = eigenvectors[2]
    center = pcd.get_center()  # 获得中心（质心）质心是一个几何概念，通常用于描述几何图形（如点集、线段、多边形等）的中心位置。
    print('平面拟合结果为：0 = %.3f * x + %.3f * y + %.3f + %3.f' % (X[0], X[1], X[2], (-X[0]*center[0] - X[1]*center[1] - X[2]*center[2])))
    plane_cloud = plane(pcd, (X[0], X[1], X[2], (-X[0]*center[0] - X[1]*center[1] - X[2]*center[2])))  # 获得投影后的点云数据
    # ------------------ 可视化点云 -----------------
    plane_cloud.paint_uniform_color([1, 0, 0.0])  # 渲染颜色
    o3d.visualization.draw_geometries([plane_cloud])
    o3d.visualization.draw_geometries([pcd])

三、结果

1.原点云数据

2.svd分解后投影平面点云数据

四、相关数据

svd分解法：最小二乘拟合平面(python/C++版) - 知乎 (zhihu.com)

奇异值分解（SVD）：奇异值分解（SVD） - 知乎 (zhihu.com)