平面方程表达式
平面方程的一般式为：
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 pltimport numpy as npdef 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<iostream>#include <iomanip>#include<math.h>#define PAI acos(-1)using namespace std;double point[][3] = {{-2,1,2},{0,1,1},{-1,2,1},{-1,0,0}};int size = sizeof(point) / sizeof(point[0]);void matrix_inverse(double(*a)[3], double(*b)[3]);int main() {double RR[3][3] = {0};// R_T * Rdouble RR_1[3][3] = {0};double A[3][1] = {0};int i,j,k;double **R = new double*[size];for (i = 0; i < size; i++)R[i] = new double[3];double **R_T = new double*[3];for (i = 0; i < 3; i++)R_T[i] = new double[size];double **RR_1R_T = new double*[3];for (i = 0; i < 3; i++)RR_1R_T[i] = new double[size];double **Z = new double*[size];for (i = 0; i < size; i++)Z[i] = new double[1];for(i = 0; i < size; i++){R[i][0] = point[i][0];R[i][1] = point[i][1];R[i][2] = 1;R_T[0][i] = point[i][0];R_T[1][i] = point[i][1];R_T[2][i] = 1;Z[i][0] = point[i][2];}for(i = 0; i < 3; ++i)// 矩阵相乘 (k的取值来源于左边矩阵的列数或右边矩阵的行数） {for(j = 0; j < 3; ++j){RR[i][j] = 0;for(k = 0; k < size; ++k)RR[i][j] += R_T[i][k] * R[k][j];}}matrix_inverse(RR, RR_1);for(i = 0; i < 3; ++i){for(j = 0; j < size; ++j){RR_1R_T[i][j] = 0; for(k = 0; k < 3; ++k)RR_1R_T[i][j] += RR_1[i][k] * R_T[k][j];}}for(i = 0; i < 3; ++i){for(j = 0; j < 1; ++j){A[i][j] = 0;for(k = 0; k < size; ++k)A[i][j] += RR_1R_T[i][k] * Z[k][j];}}double Theta_XOY = asin(fabs(-1) / (sqrt(pow(A[0][0], 2) + pow(A[1][0], 2) + 1))) * (180 / PAI) ;double Theta_YOZ = asin(fabs(A[0][0]) / (sqrt(pow(A[0][0], 2) + pow(A[1][0], 2) + 1))) * (180 / PAI) ;double Theta_XOZ = asin(fabs(A[1][0]) / (sqrt(pow(A[0][0], 2) + pow(A[1][0], 2) + 1))) * (180 / PAI) ;cout.setf(ios::fixed);cout << "平面方程为：z=" << setprecision(2) << A[0][0] << "*x+(" << setprecision(2) << A[1][0] << ")*y+(" << setprecision(2) << A[2][0] << ")" << endl;cout << "法向量为：(" << setprecision(2) << A[0][0] << "," << setprecision(2) << A[1][0] << ", -1)" << endl;cout << "平面法向量与XOY平面夹角为：" << setprecision(3) << Theta_XOY << endl;cout << "平面法向量与YOZ平面夹角为：" << setprecision(3) << Theta_YOZ << endl;cout << "平面法向量与XOZ平面夹角为：" << setprecision(3) << Theta_XOZ << endl;return 0;}void matrix_inverse(double(*a)[3], double(*b)[3])// 矩阵求逆 {using namespace std;int i, j, k;double max, temp;// 定义一个临时矩阵tdouble t[3][3];// 将a矩阵临时存放在矩阵t[n][n]中for (i = 0; i < 3; i++)for (j = 0; j < 3; j++)t[i][j] = a[i][j];// 初始化B矩阵为单位矩阵for (i = 0; i < 3; i++)for (j = 0; j < 3; j++)b[i][j] = (i == j) ? (double)1 : 0;// 进行列主消元，找到每一列的主元for (i = 0; i < 3; i++){max = t[i][i];// 用于记录每一列中的第几个元素为主元k = i;// 寻找每一列中的主元元素for (j = i + 1; j < 3; j++){if (fabs(t[j][i]) > fabs(max)){max = t[j][i];k = j;}}//cout<<"the max number is "<<max<<endl;// 如果主元所在的行不是第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 "<<temp<<endl;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];}}}}}
