1、算法过程

设拟合出的平面方程为：
a x + b y + c z + d = 0 (1) ax+by+cz+d=0\tag{1} ax+by+cz+d=0(1)

约束条件为： a 2 + b 2 + c 2 = 1 (2) a^2+b^2+c^2=1\tag{2} a2+b2+c2=1(2)

可以得到平面参数 a 、 b 、 c 、 d a、b、c、d a、b、c、d。此时，要使获得的拟合平面是最佳的，就是使点到该平面的距离的平方和最小，即满足：
e = ∑ i = 1 n d i 2 → m i n (3) e=\sum_{i=1}^nd_i^2\rightarrow min\tag{3} e=i=1∑ndi2→min(3)

式中， d i d_i di是点云数据中的任一点 p i ( x i , y i , z i ) p_i(x_i,y_i,z_i) pi(xi,yi,zi)到这个平面的距离 d i = ∣ a x i + b y i + c z i + d ∣ d_i=|ax_i+by_i+cz_i+d| di=∣axi+byi+czi+d∣。要使 e → m i n e\rightarrow min e→min，可以用 S V D SVD SVD矩阵分解得到。

推导过程如下：

所有点的平均坐标为 ( x ˉ . y ˉ , z ˉ ) (\bar{x}.\bar{y},\bar{z}) (xˉ.yˉ,zˉ),则：
a x ˉ + b y ˉ + c z ˉ + d = 0 (4) a\bar{x}+b\bar{y}+c\bar{z}+d=0\tag{4} axˉ+byˉ+czˉ+d=0(4)

式(1)与式(4)相减得：
a ( x i − x ˉ ) + b ( y i − y ˉ ) + c ( z i − z ˉ ) = 0 (5) a(x_i-\bar{x})+b(y_i-\bar{y})+c(z_i-\bar{z})=0\tag{5} a(xi−xˉ)+b(yi−yˉ)+c(zi−zˉ)=0(5)

假设矩阵：
A = [ x 1 − x ˉ y 1 − y ˉ z 1 − z ˉ x 2 − x ˉ y 2 − y ˉ z 2 − z ˉ x 3 − x ˉ y 3 − y ˉ z 3 − z ˉ ⋯ x n − x ˉ y n − y ˉ z n − z ˉ ] A=\left[ \begin{matrix} x_1-\bar{x} & y_1-\bar{y} & z_1-\bar{z} \\ x_2-\bar{x} & y_2-\bar{y} & z_2-\bar{z} \\ x_3-\bar{x} & y_3-\bar{y} & z_3-\bar{z} \\ & \cdots & \\ x_n-\bar{x} & y_n-\bar{y} & z_n-\bar{z} \\ \end{matrix} \right] A= x1−xˉx2−xˉx3−xˉxn−xˉy1−yˉy2−yˉy3−yˉ⋯yn−yˉz1−zˉz2−zˉz3−zˉzn−zˉ

列矩阵：
X = [ a b c ] X=\left[ \begin{matrix} a\\ b \\ c \\ \end{matrix} \right] X= abc

则式(5)等价于：
A X = 0 (6) AX=0\tag{6} AX=0(6)

理想情况下所有点都在平面上,式(6)成立；实际情况下有部分点在平面外，拟合的目的为平面距离所有点的距离之和尽量小，所以目标函数为：
m i n ∣ ∣ A X ∣ ∣ (7) min||AX||\tag{7} min∣∣AX∣∣(7)

约束条件为：

∣ ∣ X ∣ ∣ = 1 (8) ||X||=1\tag{8} ∣∣X∣∣=1(8)

若 A A A可做奇异值分解：
A = U D V T (9) A=UDV^T\tag{9} A=UDVT(9)

则：
∣ ∣ A X ∣ ∣ = ∣ ∣ U D V T X ∣ ∣ = ∣ ∣ D V T X ∣ ∣ (10) ||AX||=||UDV^TX||=||DV^TX||\tag{10} ∣∣AX∣∣=∣∣UDVTX∣∣=∣∣DVTX∣∣(10)

其中： V T X V^TX VTX为列矩阵，并且：
∣ ∣ V T X ∣ ∣ = ∣ ∣ X ∣ ∣ = 1 (11) ||V^TX||=||X||=1\tag{11} ∣∣VTX∣∣=∣∣X∣∣=1(11)

因为 D D D的对角元素为奇异值，假设最后一个对角元素为最小奇异值，则当且仅当：
V T X = [ 0 0 0 ⋯ 1 ] (12) V^TX=\left[ \begin{matrix} 0 \\ 0 \\ 0 \\ \cdots \\ 1\\ \end{matrix} \right]\tag{12} VTX= 000⋯1 (12)

时，式(10)可以取得最小值，即式(7)成立。此时：
X = V [ 0 0 0 ⋯ 1 ] = [ v 1 v 2 v 3 ⋯ v n ] [ 0 0 0 ⋯ 1 ] (13) X=V\left[ \begin{matrix} 0 \\ 0 \\ 0 \\ \cdots \\ 1\\ \end{matrix} \right] =\left[ \begin{matrix} v_1 & v_2 & v_3 & \cdots& v_n \\ \end{matrix} \right] \left[ \begin{matrix} 0 \\ 0 \\ 0 \\ \cdots \\ 1\\ \end{matrix} \right]\tag{13} X=V 000⋯1 =[v1v2v3⋯vn] 000⋯1 (13)

目标函数(7)在约束条件(8)下的最优解为：
X = ( a , b , c ) = ( v n , 1 , v n , 2 , v n , 3 ) (14) X=(a,b,c)=(v_{n,1},v_{n,2},v_{n,3})\tag{14} X=(a,b,c)=(vn,1,vn,2,vn,3)(14)

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

2、示例代码

python 复制代码

import numpy as np


def svd_fit_plane(pts):
    """
    最小二乘拟合平面的svd分解法
    :param pts: 三维点集合
    :return: 平面法向量res
    """
    n = points.shape[0]  # 点的个数
    A = np.ones((n, 3))  #
    # 构建计算所需矩阵
    A[:, 0] = pts[:, 0] - np.mean(pts[:, 0], axis=0)
    A[:, 1] = pts[:, 1] - np.mean(pts[:, 1], axis=0)
    A[:, 3] = pts[:, 2] - np.mean(pts[:, 2], axis=0)

    [V, _, _] = np.linalg.svd(A)

    A = V[0, 2]
    B = V[1, 2]
    C = V[2, 2]

    return A, B, C

最小二乘拟合平面——SVD分解法

目录

1、算法过程

2、示例代码