整体最小二乘拟合平面
目录
- 1、算法过程
- 2、参考文献
- 3、算法伪码
- 4、算法效果
本文由CSDN点云侠原创，原文链接。
1、算法过程
最小二乘拟合平面认为点云数据系数矩阵不存在误差，然而由于观测条件的限制，观测向量、系数矩阵都有可能存在误差，那么最小二乘方法就不再是最优的，而是有偏的，因此，可以采用总体最小二乘法拟合点云平面，并通过奇异值分解法解算拟合平面的参数。
假设对某一平面扫描得到 n n n个点的坐标为 ( x i ， y i ， z i ) ( x_i，y_i，z_i) (xi，yi，zi) ，考虑到观测数据在 x 、 y 、 z x、y、z x、y、z方向上均存在误差，则将平面方程 z + V = a x + b y + c z+V=ax+by+c z+V=ax+by+c改写为：
z i + V z i = a ( x i + V x i ) + b ( y i + V y i ) + c (1) z_i+V_{z_i}=a(x_i+V_{x_i})+b(y_i+V_{y_i})+c\tag{1} zi+Vzi=a(xi+Vxi)+b(yi+Vyi)+c(1)
式中， V x 、 V y 、 V z V_x、V_y、V_z Vx、Vy、Vz分别为 x 、 y 、 z x、y、z x、y、z方向上的改正数，将上式整理可得
( A + E A ) X = L + E L (2) (A+E_A)X=L+E_L\tag{2} (A+EA)X=L+EL(2)
式中，
A = [ x 1 y 1 1 x 2 y 2 1 ⋮ ⋮ ⋮ x n y n 1 ] , E A = [ V x 1 V y 1 1 V x 2 V y 2 1 ⋮ ⋮ ⋮ V x n V y n 1 ] , X = [ a b c ] , L = [ z 1 z 2 ⋮ z n ] , E L = [ V z 1 V z 2 ⋮ V z n ] A=\left[ \begin{matrix} x_1&y_1&1\\ x_2&y_2&1\\ \vdots&\vdots&\vdots\\ x_n&y_n&1\\ \end{matrix} \right], E_A=\left[ \begin{matrix} V_{x_1}&V_{y_1}&1\\ V_{x_2}&V_{y_2}&1\\ \vdots&\vdots&\vdots\\ V_{x_n}&V_{y_n}&1\\ \end{matrix} \right], X=\left[ \begin{matrix} a\\ b\\ c\\ \end{matrix} \right], L=\left[ \begin{matrix} z_1\\ z_2\\ \vdots\\ z_n\\ \end{matrix} \right], E_L=\left[ \begin{matrix} V_{z_1}\\ V_{z_2}\\ \vdots\\ V_{z_n}\\ \end{matrix} \right] A= x1x2⋮xny1y2⋮yn11⋮1 ,EA= Vx1Vx2⋮VxnVy1Vy2⋮Vyn11⋮1 ,X= abc ,L= z1z2⋮zn ,EL= Vz1Vz2⋮Vzn
将式(2) 表示为误差方程式的形式为：
( A + E A ) X = L + e (3) (A+E_A)X=L+e\tag{3} (A+EA)X=L+e(3)
可以改写为
[ A + E A L + e ] [ X − 1 ] = ( [ A L ] + E ) [ X − 1 ] (4) \left[ \begin{matrix} A+E_A &L+e\\ \end{matrix} \right]\left[ \begin{matrix} X\\ -1\\ \end{matrix} \right]= \left([ \begin{matrix} A&L]+E\\ \end{matrix} \right) \left[ \begin{matrix} X\\ -1\\ \end{matrix} \right] \tag{4} [A+EAL+e][X−1]=([AL]+E)[X−1](4)
式中， E = [ E A e ] E=\left[ \begin{matrix} E_A &e\\ \end{matrix} \right] E=[EAe]，用其表示的限制约束条件为：
t r ( E E T ) = t r ( E A E A T + e e T ) = v e c ( E A ) T v e c ( E A ) + e T e = m i n (5) tr(EE^T)=tr(E_AE_A^T+ee^T)=vec(E_A)^Tvec(E_A)+e^Te=min\tag{5} tr(EET)=tr(EAEAT+eeT)=vec(EA)Tvec(EA)+eTe=min(5)
对增广矩阵 [ A L ] \left[ \begin{matrix} A &L\\ \end{matrix} \right] [AL]进行奇异值分解
[ A L ] = [ U 1 U 2 ] [ ∑ 0 ] V T = U 1 ∑ V T (6) \left[ \begin{matrix} A &L\\ \end{matrix} \right]= \left[ \begin{matrix} U_1 &U_2\\ \end{matrix} \right] \left[ \begin{matrix} \sum \\0\\ \end{matrix} \right]V^T=U_1\sum V^T\tag{6} [AL]=[U1U2][∑0]VT=U1∑VT(6)
式中， V T = [ V 11 V 12 V 21 V 22 ] , U 1 = [ U 11 U 12 ] , ∑ = [ ∑ 1 0 0 ∑ 2 ] V 12 = [ v 1 , m + 1 v 2 , m + 1 ⋯ v m , m + 1 ] T , V 22 = [ v m + 1 , m + 1 ] V^T= \left[ \begin{matrix} V_{11} &V_{12}\\ V_{21} &V_{22}\\ \end{matrix} \right],U_1=\left[ \begin{matrix} U_{11} &U_{12}\\ \end{matrix} \right],\sum= \left[ \begin{matrix} \sum_1 &0\\ 0 &\sum_2\\ \end{matrix} \right]\\ V_{12}=\left[ \begin{matrix} v_{1,m+1} &v_{2,m+1} &\cdots &v_{m,m+1} \\ \end{matrix} \right]^T,V_{22}=\left[ \begin{matrix} v_{m+1,m+1}\\ \end{matrix} \right] VT=[V11V21V12V22],U1=[U11U12],∑=[∑100∑2]V12=[v1,m+1v2,m+1⋯vm,m+1]T,V22=[vm+1,m+1]
由矩阵逼近理论可知，参数的总体最小二乘估值为
V = − V 12 V 22 − 1 (7) V=-V_{12}V_{22}^{-1}\tag{7} V=−V12V22−1(7)
由于 v i v_i vi为矩阵 [ A L ] T [ A L ] \left[ \begin{matrix} A&L\\ \end{matrix} \right]^T\left[ \begin{matrix} A&L\\ \end{matrix} \right] [AL]T[AL]的特征向量，所以奇异值与特征向量满足关系
[ A T A A T L L T A L T L ] [ X − 1 ] = δ m + 1 2 [ X − 1 ] (8) \left[ \begin{matrix} A^TA&A^TL\\ L^TA&L^TL\\ \end{matrix} \right] \left[ \begin{matrix} X\\ -1\\ \end{matrix} \right]=\delta_{m+1}^2\left[ \begin{matrix} X\\ -1\\ \end{matrix} \right] \tag{8} [ATALTAATLLTL][X−1]=δm+12[X−1](8)
进一步可以计算得到单位权方差为
δ 0 2 ( T L S ) = δ m + 1 2 / ( n − m ) (9) \delta_{0}^2(TLS)=\delta_{m+1}^2/(n-m)\tag{9} δ02(TLS)=δm+12/(n−m)(9)
2、参考文献
[1] 薄怀志.三种点云数据平面拟合方法的精度比较与分析[J].测绘与空间地理信息,2018,41(05):206-208.
3、算法伪码
import numpy as np
def global_lsq_fit_plane(points):
"""
全局最小二乘拟合平面
:param points: 三维点集合
:return: 平面法向量res
"""
n = points.shape[0] # 点的个数
AL = np.ones((n, 4)) #
# 构建计算所需矩阵
for i in range(0, n):
AL[i, 0] = points[i].x
AL[i, 1] = points[i].y
AL[i, 3] = points[i].z
[S, V, D] = np.linalg.svd(AL)
v_12 = V[1, 2]
v_22 = V[2, 2]
X = -v_12 / v_22
return X
4、算法效果
