最小二乘法拟合平面(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 CAxCByCD=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} x1x2xny1y2yn111 abc = z1z2zn
得到 R ⋅ A = Y R \cdot A = Y RA=Y的矩阵形式后,便可利用 A = ( R T ⋅ R ) − 1 ⋅ R T ⋅ Y A = (R^T \cdot R)^{-1} \cdot R^T \cdot Y A=(RTR)1RTY得到 a 、 b 、 c a、b、c abc的值。
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)

结果为:
最小二乘法拟合平面(Python&C++实现)_第1张图片
C++完整代码为:

#include
#include 
#include
#define PAI acos(-1)

using namespace std;

double a[][3] = {{1, 0, 2}, {1, 1, 2}, {0, 1, 2}, {0, 0, 2}};
int M = sizeof(a) / sizeof(a[0]);

void matrix_inverse(double(*a)[3], double(*b)[3]);
	
int main() 
{
	double R[M][3] = {0};
	double R_T[3][M] = {0};
	double RR[3][3] = {0};	// R_T * R
	double RR_1[3][3] = {0};
	double RR_1R_T[3][M] = {0};	// RR_1 * R_T
	double Z[M][1] = {0};
	double A[3][1] = {0};
	int i, j;
	
	for(int i = 0; i < M; i++)
	{
		R[i][0] = a[i][0];
		R[i][1] = a[i][1];
		R[i][2] = 1;
		
		R_T[0][i] = a[i][0];
		R_T[1][i] = a[i][1];
		R_T[2][i] = 1;
		
		Z[i][0] = a[i][2];
	}
	
	for(int i = 0; i < 3; ++i)		// 矩阵相乘 (k的取值来源于左边矩阵的列数或右边矩阵的行数) 
		for(int j = 0; j < 3; ++j)
		{
			RR[i][j] = 0;
			for(int k = 0; k < M; ++k)
				RR[i][j] += R_T[i][k] * R[k][j];
		}
			
	matrix_inverse(RR, RR_1);
	
	for(int i = 0; i < 3; ++i)		// 矩阵相乘 (k的取值来源于左边矩阵的列数或右边矩阵的行数) 
		for(int j = 0; j < M; ++j)
		{
			RR_1R_T[i][j] = 0; 
			for(int k = 0; k < 3; ++k)
				RR_1R_T[i][j] += RR_1[i][k] * R_T[k][j];
		}
				
	for(int i = 0; i < 3; ++i)		// 矩阵相乘 (k的取值来源于左边矩阵的列数或右边矩阵的行数) 
		for(int j = 0; j < 1; ++j)
		{
			A[i][j] = 0;
			for(int k = 0; k < M; ++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 "<
		// 如果主元所在的行不是第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];
				}
			}
		}
	}
}

你可能感兴趣的:(python,最小二乘法,平面)