目录
- 一、算法原理
- 二、代码实现
- 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('最小二乘拟合平面');
三、算法效果
