最小二乘拟合平面——直接求解法

目录

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

三、算法效果