1、算法过程

最小二乘拟合平面认为点云数据系数矩阵不存在误差，然而由于观测条件的限制，观测向量、系数矩阵都有可能存在误差，那么最小二乘方法就不再是最优的，而是有偏的，因此，可以采用总体最小二乘法拟合点云平面，并通过奇异值分解法解算拟合平面的参数。
假设对某一平面扫描得到 $n$ 个点的坐标为 $x_i，y_i，z_i)$ ，考虑到观测数据在 $x 、 y 、 z$ 方向上均存在误差，则将平面方程 $z + V = a x + b y + c$ 改写为：
$z_i+V_{z_i}=a(x_i+V_{x_i})+b(y_i+V_{y_i})+c\tag{1}$
式中， $V_x、V_y、V_z$ 分别为 $x 、 y 、 z$ 方向上的改正数，将上式整理可得
$(A+E_A)X=L+E_L\tag{2}$
式中，
$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]$
将式(2) 表示为误差方程式的形式为：
$(A+E_A)X=L+e\tag{3}$
可以改写为

$\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}$

式中， $E=\left[ \begin{matrix} E_A &e\\ \end{matrix} \right]$ ，用其表示的限制约束条件为：
$tr(EE^T)=tr(E_AE_A^T+ee^T)=vec(E_A)^Tvec(E_A)+e^Te=min\tag{5}$
对增广矩阵 $\left[ \begin{matrix} A &L\\ \end{matrix} \right]$ 进行奇异值分解

$\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}$
式中， $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]$
由矩阵逼近理论可知，参数的总体最小二乘估值为
$V=-V_{12}V_{22}^{-1}\tag{7}$

由于 $v_i$ 为矩阵 $\left[ \begin{matrix} A&L\\ \end{matrix} \right]^T\left[ \begin{matrix} A&L\\ \end{matrix} \right]$ 的特征向量，所以奇异值与特征向量满足关系
$\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}$
进一步可以计算得到单位权方差为
$\delta_{0}^2(TLS)=\delta_{m+1}^2/(n-m)\tag{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、算法效果

在这里插入图片描述

整体最小二乘拟合平面

目录

1、算法过程

2、参考文献

3、算法伪码

4、算法效果

猜你喜欢