最小二乘法拟合平面（Python&C++实现）

陆先森不怕鬼

已于 2023-03-24 09:28:43 修改

阅读量1.6k

点赞数 5

文章标签： python 最小二乘法平面

于 2022-12-16 16:09:10 首次发布

本文链接：https://blog.csdn.net/weixin_44729155/article/details/128343928

版权

平面方程表达式

平面方程的一般式为：
$A x + B y + C z + D = 0$
通过变形可表示为：
$-\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C}=z$
既然A、B、C均未知，那么平面方程也可表示为：
$ax+by+c=z\tag{1}$

最小二乘法拟合

现存在一组点 $[x_1, y_1, z_1], [x_2, y_2, z_2], \ldots, [x_n, y_n, z_n]$ ，要对其所在的平面拟合，将第一组点代入 $(1)$ 式后可表示为：
$ax_1+by_1+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}$
若将全部点代入，则有
$\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}$
得到 $\cdot A = Y$ 的矩阵形式后，便可利用 $(R^T \cdot R)^{-1} \cdot R^T \cdot Y$ 得到 $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<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 * R
	double 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;
	// 定义一个临时矩阵t
	double 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];
				}
			}
		}
	}
}