整体最小二乘拟合平面

目录

  • 1、算法过程
  • 2、参考文献
  • 3、算法伪码
  • 4、算法效果

整体最小二乘拟合平面_第1张图片

本文由CSDN点云侠原创,原文链接。

1、算法过程

  最小二乘拟合平面认为点云数据系数矩阵不存在误差,然而由于观测条件的限制,观测向量、系数矩阵都有可能存在误差,那么最小二乘方法就不再是最优的,而是有偏的,因此,可以采用总体最小二乘法拟合点云平面,并通过奇异值分解法解算拟合平面的参数。
  假设对某一平面扫描得到 n n n个点的坐标为 ( x i , y i , z i ) ( x_i,y_i,z_i) (xiyizi) ,考虑到观测数据在 x 、 y 、 z x、y、z xyz方向上均存在误差,则将平面方程 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 VxVyVz分别为 x 、 y 、 z x、y、z xyz方向上的改正数,将上式整理可得
( 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= x1x2xny1y2yn111 ,EA= Vx1Vx2VxnVy1Vy2Vyn111 ,X= abc ,L= z1z2zn ,EL= Vz1Vz2Vzn
将式(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][X1]=([AL]+E)[X1](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=U1VT(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],=[1002]V12=[v1,m+1v2,m+1vm,m+1]T,V22=[vm+1,m+1]
  由矩阵逼近理论可知,参数的总体最小二乘估值为
V = − V 12 V 22 − 1 (7) V=-V_{12}V_{22}^{-1}\tag{7} V=V12V221(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][X1]=δm+12[X1](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/(nm)(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、算法效果

整体最小二乘拟合平面_第2张图片

你可能感兴趣的:(CloudCompare,平面,线性代数,算法,开发语言,计算机视觉,3d)