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

一、算法原理

平面方程的一般表达式为：
$Ax+By+Cz+D=0(C\neq0)\tag{1}$
即：
$z=-\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C}\tag{2}$
记：
$a_0=-\frac{A}{C},a_1=-\frac{B}{C},a_2=-\frac{D}{C}\tag{3}$
将式(3)代入式(2)可得式(4)：
$z=a_0x+a_1y+a_2\tag{4}$
对于一系列 $n$ 个点 $(n\geq3)$ ； $x_i,y_i,z_i),i=0,1,...,n-1$ ，要用该 $n$ 个点拟合平面方程，即使：
$S=\sum_{i=1}^n(a_0x+a_1y+a_2 - z) \rightarrow min\tag{5}$

要使 $S$ 最小，应将式(4)两边对 $a_0,a_1,a_2$ 求偏导，并且令偏导数为零。
即：
$\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}$
改写成矩阵的形式为：
$\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}$
解方程组(7)，即可得到参数 $a_0,a_1,a_2$ ，代入式(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('最小二乘拟合平面');