最小二乘法计算平面度

在已知平面内数据点的三维坐标的情况下，求解平面方程的参数。已知平面的方程式为：
$$z=ax+by+c$$
通过深度相机检测到平面上的$n$个点云数据$p(x_i,y_i,z_i)$。通过最小二乘法，可以求解平面的参数：$a,b,c$，进而求解平面度。

1. 定义单个数据点的误差项：${\delta }_i=a{x}_i+b{y}_i-z_i+c$
2. 对于$n$组数据，定义所有数据的最小二乘问题：$F(a,b,c)={\Sigma }_{i=1}^n{\delta }_i^2={\Sigma }_{i=1}^n(a{x}_i+b{y}_i-z_i+c{)}^2$
3. 平面方程的自变量为$a,b,c$且不相关，让$F(a,b,c)$分别对$a,b,c$求导，并使其为0。

$$\begin{gathered} \{\begin{array}{rl}& \frac{\Delta F}{\Delta a}={\Sigma }_{i=1}^{n}2x_i(a{x}_i+b{y}_i-z_i+c)=0\\ & \frac{\Delta F}{\Delta b}={\Sigma }_{i=1}^{n}2y_i(a{x}_i+b{y}_i-z_i+c)=0\\ & \frac{\Delta F}{\Delta c}={\Sigma }_{i=1}^{n}2(a{x}_i+b{y}_i-z_i+c)=0\end{array} \\ \Downarrow \\ 将a,b,c写到方程的一边(便于写成矩阵形式进行求解) \\ \{\begin{array}{rl}& a{\Sigma }_{i=1}^n{x}_i^2+b{\Sigma }_{i=1}^n{x}_i{y}_i+c{\Sigma }_{i=1}^n{x}_i={\Sigma }_{i=1}^n{x}_i{z}_i\\ & a{\Sigma }_{i=1}^n{x}_i{y}_i+b{\Sigma }_{i=1}^n{y}_i^2+c{\Sigma }_{i=1}^n{y}_i={\Sigma }_{i=1}^n{y}_i{z}_i\\ & a{\Sigma }_{i=1}^n{x}_i+b{\Sigma }_{i=1}^n{y}_i+nc={\Sigma }_{i=1}^n{z}_i\end{array} \\ \Downarrow \\ \left[\begin{array}{ccc}\Sigma x_i^2& \Sigma x_i{y}_i& \Sigma x_i\\ \Sigma x_i{y}_i& \Sigma y_i^2& \Sigma y_i\\ \Sigma x_i& \Sigma y_i& n\end{array}\right]\left[\begin{array}{c}a\\ b\\ c\end{array}\right]=\left[\begin{array}{c}\Sigma x_i{z}_i\\ \Sigma y_i{z}_i\\ \Sigma z_i\end{array}\right] \end{gathered}$$

此后就是求解$Ax=b$的问题。

#-- coding:UTF-8 --
#空间平面的方程:z = ax + by + c
#目标：求解a,b,c的值
#已知条件：12个被测点的三维坐标
import numpy as np
p=[[0,0,3.52],
   [1,0,2.3],
   [2,0,-2.69],
   [0,1,-2.65],
   [1,1,3.25],
   [2,1,4.36],
   [0,2,-2.36],
   [1,2,4.56],
   [2,2,-3.54],
   [0,3,2.36],
   [1,3,-5.65],
   [2,3,2.59]
   ]
n = len(p)
B=[0,0,0]
A = np.zeros((3,3),np.float32)
for i in range(n):
    B[0] = B[0] + p[i][0]*p[i][2]
    B[1] = B[1] + p[i][1]*p[i][2]
    B[2] = B[2] + p[i][2]
    A[0][0] = A[0][0] + p[i][0]**2
    A[0][1] = A[0][1] + p[i][0]*p[i][1]
    A[0][2] = A[0][2] + p[i][0]
    A[1][0] = A[0][1]
    A[1][1] = A[1][1] + p[i][1]**2
    A[1][2] = A[1][2] + p[i][1]
    A[2][0] = A[0][2]
    A[2][1] = A[1][2]
    A[2][2] = n
# 【1】计算拟合平面的系数a,b,c
x = np.matmul(np.linalg.inv(A),B)
print(B)
print(A)
print(x)

# 【2】计算平面度
# 计算被一个被测点的实际高度Z与理想高度Zs之间的差值
# 平面度的值即为被测点最大差值与最小差值的差
delta = np.zeros((n,),np.float32)
for i in range(n):
    delta[i] = p[i][2] - (x[0]*p[i][0]+x[1]*p[i][1]+x[2])
print(delta)
flatness = max(delta) - min(delta)
print(flatness)

[5.899999999999999, 0.17999999999999616, 6.049999999999999]
[[20. 18. 12.]
 [18. 42. 18.]
 [12. 18. 12.]]
[-0.01875    -0.59300001  1.41241658]
[ 2.1075835   0.90633345 -4.0649166  -3.4694166   2.4493334   3.5780835
 -2.5864165   4.3523335  -3.7289166   2.7265835  -5.2646666   2.9940834 ]
9.617001