目录
一、算法原理
平面方程的一般表达式为：
A x + B y + C z + D = 0 ( C ≠ 0 ) (1) Ax+By+Cz+D=0(C\neq0)\tag{1} Ax+By+Cz+D=0(C=0)(1)
即：
z = − A C x − B C y − D C (2) z=-\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C}\tag{2} z=−CAx−CBy−CD(2)
记：
a 0 = − A C , a 1 = − B C , a 2 = − D C (3) a_0=-\frac{A}{C},a_1=-\frac{B}{C},a_2=-\frac{D}{C}\tag{3} a0=−CA,a1=−CB,a2=−CD(3)
将式(3)代入式(2)可得式(4)：
z = a 0 x + a 1 y + a 2 (4) z=a_0x+a_1y+a_2\tag{4} z=a0x+a1y+a2(4)
对于一系列 n n n个点 ( n ≥ 3 ) (n\geq3) (n≥3)； ( x i , y i , z i ) , i = 0 , 1 , . . . , n − 1 (x_i,y_i,z_i),i=0,1,...,n-1 (xi,yi,zi),i=0,1,...,n−1，要用该 n n n个点拟合平面方程，即使：
S = ∑ i = 1 n ( a 0 x + a 1 y + a 2 − z ) → m i n (5) S=\sum_{i=1}^n(a_0x+a_1y+a_2 - z) \rightarrow min\tag{5} S=i=1∑n(a0x+a1y+a2−z)→min(5)
要使 S S S最小，应将式(4)两边对 a 0 , a 1 , a 2 a_0,a_1,a_2 a0,a1,a2求偏导，并且令偏导数为零。
即：
{ 2 ∑ i = 1 n ( a 0 x i + a 1 y i + a 2 − z i ) x i = 0 2 ∑ i = 1 n ( a 0 x i + a 1 y i + a 2 − z i ) y i = 0 2 ∑ i = 1 n ( a 0 x i + a 1 y i + a 2 − z i ) = 0 (6) \begin{cases} 2\sum_{i=1}^n(a_0\ x_i+a_1\ y_i+a_2-z_i)x_i=0\\ 2\sum_{i=1}^n(a_0\ x_i+a_1\ y_i+a_2-z_i)y_i=0\\ 2\sum_{i=1}^n(a_0\ x_i+a_1\ y_i+a_2-z_i)=0 \end{cases} \tag{6} ⎩ ⎨ ⎧2∑i=1n(a0 xi+a1 yi+a2−zi)xi=02∑i=1n(a0 xi+a1 yi+a2−zi)yi=02∑i=1n(a0 xi+a1 yi+a2−zi)=0(6)
改写成矩阵的形式为：
∑ i = 1 n x i 2 ∑ i = 1 n x i y i ∑ i = 1 n x i ∑ i = 1 n x i y i ∑ i = 1 n y i 2 ∑ i = 1 n y i ∑ i = 1 n x i ∑ i = 1 n y i n \] \[ a 0 a 1 a 2 \] = \[ ∑ i = 1 n x i z i ∑ i = 1 n y i z i ∑ i = 1 n z i \] (7) \\left\[ \\begin{matrix} \\sum_{i=1}\^n\\ x_{i}\^{2}\&\\sum_{i=1}\^n\\ x_{i}\\ y_{i}\&\\sum_{i=1}\^n\\ x_{i} \\\\ \\sum_{i=1}\^n\\ x_{i}\\ y_{i}\&\\sum_{i=1}\^n\\ y_{i}\^{2}\&\\sum_{i=1}\^n\\ y_{i} \\\\ \\sum_{i=1}\^n\\ x_{i}\\ \&\\sum_{i=1}\^n y_{i} \& n\\\\ \\end{matrix} \\right\]\\left\[ \\begin{matrix} a_0\\\\ a_1\\\\ a_2\\\\ \\end{matrix} \\right\] =\\left\[ \\begin{matrix} \\sum_{i=1}\^n\\ x_{i}\\ z_{i}\\\\ \\sum_{i=1}\^n\\ y_{i}\\ z_{i}\\\\ \\sum_{i=1}\^n\\ z_{i}\\\\ \\end{matrix} \\right\]\\tag{7} ∑i=1n xi2∑i=1n xi yi∑i=1n xi ∑i=1n xi yi∑i=1n yi2∑i=1nyi∑i=1n xi∑i=1n yin a0a1a2 = ∑i=1n xi zi∑i=1n yi zi∑i=1n zi (7) 解方程组(7)，即可得到参数 a 0 , a 1 , a 2 a_0,a_1,a_2 a0,a1,a2，代入式(4)即可求得平面方程。 # 二、代码实现 ## 1、python ```python import numpy as np import matplotlib.pyplot as plt # 创建函数，用于生成不同属于一个平面的100个离散点 def not_all_in_plane(a, b, c): x = np.random.uniform(-10, 10, size=100) y = np.random.uniform(-10, 10, size=100) z = (a * x + b * y + c) + np.random.normal(-1, 1, size=100) return x, y, z # 调用函数，生成离散点 x, y, z = not_all_in_plane(2, 5, 6) # ------------------------构建系数矩阵----------------------------- 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])) # -------------------------结果展示------------------------------- fig1 = plt.figure() ax1 = fig1.add_subplot(111, projection='3d') ax1.set_xlabel("x") ax1.set_ylabel("y") ax1.set_zlabel("z") ax1.scatter(x, y, z, c='r', marker='o') x_p = np.linspace(-10, 10, 100) y_p = np.linspace(-10, 10, 100) x_p, y_p = np.meshgrid(x_p, y_p) z_p = X[0] * x_p + X[1] * y_p + X[2] ax1.plot_wireframe(x_p, y_p, z_p, rstride=10, cstride=10) plt.show() ``` ## 2、matlab ```cpp clc;clear; %% -------------------------------读取点云--------------------------------- pc = ReadPointCloud('plane1.pcd'); %% -----------------------------获取点云信息------------------------------- n ; % 点的个数 x ; % 点的x坐标 y ; % 点的y坐标 z ; % 点的z坐标 %% -------------------------------拟合平面--------------------------------- % 矩阵M M = [sumXX sumXY sumX; sumXY sumYY sumY; sumX sumY n]; % 矩阵N N = [sumXZ sumYZ sumZ]'; % 求解 X = pinv(M)*N; a = X(1);b = X(2);c = X(3); %% ---------------------------可视化拟合结果------------------------------- figure % 图形绘制 scatter3(x,y,z,'filled') hold on; [XFit,YFit]= meshgrid (xfit,yfit); ZFit = a * XFit + b * YFit + c; mesh(XFit,YFit,ZFit); title('最小二乘拟合平面'); ``` # 三、算法效果 