最小二乘法拟合平面（Python&C++实现）
平面方程表达式
平面方程的一般式为：
A x + B y + C z + D = 0 Ax+By+Cz+D=0 Ax+By+Cz+D=0
通过变形可表示为：
− A C x − B C y − D C = z -\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C}=z −CAx−CBy−CD=z
既然A、B、C均未知，那么平面方程也可表示为：
a x + b y + c = z (1) ax+by+c=z\tag{1} ax+by+c=z(1)
最小二乘法拟合
现存在一组点 [ x 1 , y 1 , z 1 ] , [ x 2 , y 2 , z 2 ] , … , [ x n , y n , z n ] [x_1, y_1, z_1], [x_2, y_2, z_2], \ldots, [x_n, y_n, z_n] [x1,y1,z1],[x2,y2,z2],…,[xn,yn,zn]，要对其所在的平面拟合，将第一组点代入 ( 1 ) (1) (1)式后可表示为：
a x 1 + b y 1 + c = z 1 ax_1+by_1+c=z_1 ax1+by1+c=z1
用矩阵可表示为：
[ x 1 y 1 1 ] [ a b c ] = [ z 1 ] \begin{gathered} \quad \begin{bmatrix} x_1 & y_1 & 1 \end{bmatrix} \begin{bmatrix} a \\ b \\ c \end{bmatrix} = \begin{bmatrix} z_1 \end{bmatrix} \end{gathered} [x1y11] abc =[z1]
若将全部点代入，则有
[ x 1 y 1 1 x 2 y 2 1 … … … x n y n 1 ] [ a b c ] = [ z 1 z 2 … z n ] \begin{gathered} \quad \begin{bmatrix} x_1 & y_1 & 1 \\ x_2 & y_2 & 1 \\ \ldots & \ldots & \ldots \\ x_n & y_n & 1 \end{bmatrix} \begin{bmatrix} a \\ b \\ c \end{bmatrix} = \begin{bmatrix} z_1 \\ z_2 \\ \ldots \\ z_n \end{bmatrix} \end{gathered} x1x2…xny1y2…yn11…1 abc = z1z2…zn
得到 R ⋅ A = Y R \cdot A = Y R⋅A=Y的矩阵形式后，便可利用 A = ( R T ⋅ R ) − 1 ⋅ R T ⋅ Y A = (R^T \cdot R)^{-1} \cdot R^T \cdot Y A=(RT⋅R)−1⋅RT⋅Y得到 a 、 b 、 c a、b、c a、b、c的值。
Python完整代码为：
import matplotlib.pyplot as plt
import numpy as np
def Fit_face(x=None, y=None, z=None):
if x is None:
x = [1, 1, 0, 0]
if y is None:
y = [0, 1, 1, 0]
if z is None:
z = [1, 1, 1, 1]
R = []
for i in range(len(x)):
R.append([x[i], y[i], 1])
R = np.mat(R)
A = np.dot(np.dot(np.linalg.inv(np.dot(R.T, R)), R.T), z)
A = np.array(A, dtype='float32').flatten()
print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f' % (A[0], A[1], A[2]))
print('法向量为：（%.3f, %.3f, -1）' % (A[0], A[1]))
Theta_XOY = np.degrees(np.arcsin(np.abs(-1) / np.sqrt(np.power(A[0], 2) + np.power(A[1], 2) + np.power(-1, 2))))
Theta_YOZ = np.degrees(np.arcsin(np.abs(A[0]) / np.sqrt(np.power(A[0], 2) + np.power(A[1], 2) + np.power(-1, 2))))
Theta_XOZ = np.degrees(np.arcsin(np.abs(A[1]) / np.sqrt(np.power(A[0], 2) + np.power(A[1], 2) + np.power(-1, 2))))
print('平面法向量与XOY平面夹角为：%.3f' % Theta_XOY)
print('平面法向量与YOZ平面夹角为：%.3f' % Theta_YOZ)
print('平面法向量与XOZ平面夹角为：%.3f' % Theta_XOZ)
# 展示图像
fig1 = plt.figure()
# ax1 = fig1.add_subplot(111, projection='3d')
ax1 = plt.axes(projection='3d')
ax1.set_xlabel("x")
ax1.set_ylabel("y")
ax1.set_zlabel("z")
ax1.grid(False) # 关闭网格
x_p = [np.min(x), np.max(x)]
y_p = [np.min(y), np.max(y)]
x_p, y_p = np.meshgrid(x_p, y_p)
z_p = A[0] * x_p + A[1] * y_p + A[2]
xx = [(np.min(x) + np.max(x)) / 2, (np.min(x) + np.max(x)) / 2 + A[0]]
yy = [(np.min(y) + np.max(y)) / 2, (np.min(y) + np.max(y)) / 2 + A[1]]
zz = [(np.min(z) + np.max(z)) / 2, (np.min(z) + np.max(z)) / 2 - 1]
x.append((np.min(x) + np.max(x)) / 2)
y.append((np.min(y) + np.max(y)) / 2)
z.append((np.min(z) + np.max(z)) / 2)
ax1.scatter(x, y, z, c='r', marker='o') # 散点图
for i in range(len(x)):
ax1.text(x[i], y[i], z[i], (x[i], y[i], z[i]), c='r') # 显示点坐标
ax1.plot_wireframe(x_p, y_p, z_p, rstride=10, cstride=10) # 线框图
ax1.plot3D(xx, yy, zz, c='b', linestyle='--')
plt.show()
if __name__ == '__main__':
x = [1, 1, 0, 0]
y = [0, 1, 1, 0]
z = [2, 2, 2, 2]
Fit_face(x=x, y=y, z=z)
结果为：
C++完整代码为：
#include
// 如果主元所在的行不是第i行，则进行行交换
if (k != i)
{
// 进行行交换
for (j = 0; j < 3; j++)
{
temp = t[i][j];
t[i][j] = t[k][j];
t[k][j] = temp;
// 伴随矩阵B也要进行行交换
temp = b[i][j];
b[i][j] = b[k][j];
b[k][j] = temp;
}
}
if (t[i][i] == 0)
{
cout << "\nthe matrix does not exist inverse matrix\n";
break;
}
// 获取列主元素
temp = t[i][i];
// 将主元所在的行进行单位化处理
//cout<<"\nthe temp is "<
for (j = 0; j < 3; j++)
{
t[i][j] = t[i][j] / temp;
b[i][j] = b[i][j] / temp;
}
for (j = 0; j < 3; j++)
{
if (j != i)
{
temp = t[j][i];
//消去该列的其他元素
for (k = 0; k < 3; k++)
{
t[j][k] = t[j][k] - temp * t[i][k];
b[j][k] = b[j][k] - temp * b[i][k];
}
}
}
}
}