平面方程表达式
平面方程的一般式为:
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<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];
}
}
}
}
}