最小二乘法计算平面度

在已知平面内数据点的三维坐标的情况下，求解平面方程的参数。已知平面的方程式为：z=ax+by+cz=ax+by+cz=ax+by+c通过深度相机检测到平面上的n个点云数据。通过最小二乘法，可以求解平面的参数：a,b,ca,b,ca,b,c，进而求解平面度。本文假定测量数据为：#-- coding:UTF-8 --#空间平面的方程:z = ax + by + c#目标：求解a,b,c的值#已知条件：

Cudi_

12536人浏览 · 2020-08-14 09:30:57

Cudi_ · 2020-08-14 09:30:57 发布

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

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

$\left \{ \begin{aligned} &\frac{\Delta F}{\Delta a}=\Sigma_{i=1}^{n}2x_{i}(ax_{i}+by_{i}-z_{i}+c)=0 \\ &\frac{\Delta F}{\Delta b}=\Sigma_{i=1}^{n}2y_{i}(ax_{i}+by_{i}-z_{i}+c)=0 \\ &\frac{\Delta F}{\Delta c}=\Sigma_{i=1}^{n}2(ax_{i}+by_{i}-z_{i}+c)=0\\ \end{aligned} \right .\\ \Downarrow \\ 将a,b,c写到方程的一边(便于写成矩阵形式进行求解) \\ \left \{ \begin{aligned} &a\Sigma_{i=1}^{n}{x_{i}^2}+b\Sigma_{i=1}^{n}{x_iy_i}+c\Sigma_{i=1}^{n}{x_i}=\Sigma_{i=1}^{n}{x_iz_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_iz_i} \\ &a\Sigma_{i=1}^{n}{x_{i}}+b\Sigma_{i=1}^{n}{y_{i}}+nc=\Sigma_{i=1}^{n}{z_i}\\ \end{aligned} \right .\\ \Downarrow \\ \begin{bmatrix} \Sigma{x_{i}^2}&\Sigma{x_iy_i}&\Sigma{x_i}\\ \Sigma{x_{i}y_{i}}&\Sigma{y_{i}^{2}}&\Sigma{y_i}\\ \Sigma{x_{i}}&\Sigma{y_{i}}&n\\ \end{bmatrix} \begin{bmatrix} a\\b\\c \end{bmatrix}=\begin{bmatrix} \Sigma{x_iz_i}\\ \Sigma{y_iz_i}\\ \Sigma{z_i}\\ \end{bmatrix}$

此后就是求解 $A x = 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