最小二乘拟合平面——直接求解法
目录
- 一、算法原理
- 二、代码实现
-
- 1、python
- 2、matlab
- 三、算法效果
一、算法原理
平面方程的一般表达式为：
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
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
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('最小二乘拟合平面');
三、算法效果
