整体最小二乘拟合空间直线

本文由CSDN点云侠原创，原文链接。爬虫自重，把自己当个人。

1、算法过程

  空间直线的点向式方程为：
$$\frac{x-x_0}{m}=\frac{y-y_0}{n}=\frac{z-z_0}{p}\text{\hspace{1em}(1)}$$
式中：$m,n,p$为空间直线的方向向量；$x_0,y_0,z_0$为空间直线的已知点。

  空间中任意一点$P(x,y,z)$到空间点$Q_1(x_1,y_1,z_1)$和$Q_2(x_1,y_1,z_1)$组成的空间直线的距离为：
$$d=\frac{||(Q_1{Q}_2\times Q_{1}P)|{|}_2}{||Q_1{Q}_2|{|}_2}\text{\hspace{1em}(2)}$$

  式(1)可以变形为：

$$\{\begin{array}{l}x=\frac{m}{p}(z-z_0)+x_0\\ y=\frac{n}{p}(z-z_0)+y_0\end{array}\text{\hspace{1em}(3)}$$
  设$a=\frac{m}{p}、b=x_0-\frac{m}{p}{z}_0、c=\frac{n}{p}、d=y_0-\frac{n}{p}{z}_0$则式(1)可表示为：
$$\{\begin{array}{l}x=az+b\\ y=cz+d\end{array}\text{\hspace{1em}(4)}$$

  三维空间直线的变换方程式(4)，其矩阵形式为：

$$\left[\begin{array}{c}x\\ y\end{array}\right]=\left[\begin{array}{cccc}z& 1& 0& 0\\ 0& 0& z& 1\end{array}\right]\left[\begin{array}{c}\hat{a}\\ \hat{b}\\ \hat{c}\\ \hat{d}\end{array}\right]\text{\hspace{1em}(5)}$$

式(5)的误差方程可表示为：

$$V=\left[\begin{array}{cccc}z& 1& 0& 0\\ 0& 0& z& 1\end{array}\right]\left[\begin{array}{c}\hat{a}\\ \hat{b}\\ \hat{c}\\ \hat{d}\end{array}\right]-\left[\begin{array}{c}x\\ y\end{array}\right]\text{\hspace{1em}(6)}$$

令：
$B=\left[\begin{array}{cccc}z& 1& 0& 0\\ 0& 0& z& 1\end{array}\right],L=\left[\begin{array}{c}x\\ y\end{array}\right],\hat{X}=\left[\begin{array}{c}\hat{a}\\ \hat{b}\\ \hat{c}\\ \hat{d}\end{array}\right]$

则式(6)可以简化为：
$$V=B\hat{X}-L\text{\hspace{1em}(7)}$$

  假设三维空间直线的观测数据为$L_i(x_i,y_i,z_i)$，其误差方程可以按照式(7)表述为：
$B=\left[\begin{array}{cccc}{z}_1& 1& 0& 0\\ 0& 0& z_1& 1\\ ⋮& ⋮& ⋮& ⋮\\ z_n& 1& 0& 0\\ 0& 0& z_n& 1\end{array}\right],L=\left[\begin{array}{c}{x}_1\\ y_1\\ ⋮\\ x_n\\ y_n\end{array}\right],\hat{X}=\left[\begin{array}{c}\hat{a}\\ \hat{b}\\ \hat{c}\\ \hat{d}\end{array}\right]$

  整体最小二乘是同时顾及观测值和系数矩阵误差的一种平差方法，相比于最小二乘，其参数估值是统计最优解，但参数求解相对复杂。整体最小二乘有两种解法：奇异值分解解法和迭代解法。若利用整体最小二乘的SVD解法对误差方程式(7)进行解算，即认为误差方程矩阵$Ｂ$和$Ｌ$ 的所有元素均包含误差。而三维空间直线的误差方程式(7)的系数阵中包含了常数项和带有误差的观测量，整体最小二乘的SVD解法忽略了误差方程式(7)的特殊性。
迭代解法解算整体最小二乘空间直线的误差方程式(7)，平差准则为：
$$\sum _{i=1}^n({\hat{L}}_i-L_i{)}^2+\sum _{i=1,j=1}^{j=t,i=n}({\hat{B}}_{ij}-B_{ij}{)}^2=min\text{\hspace{1em}(8)}$$

代入式(7)，对各元素进行求导，获得迭代方程：
$$\{\begin{array}{l}{\hat{B}}^T\hat{X}={\hat{B}}^{T}L\\ N_b{\hat{B}}^T={\hat{B}}^T+\hat{X}{L}^T\end{array}\text{\hspace{1em}(9)}$$

式中：$N_b=E+\hat{X}{\hat{X}}^T$。整体最小二乘空间直线拟合的迭代解法过程如下图所示。

2、参考文献

[1] 包建强,张献州,李圆等.多种空间直线拟合方法应用分析[J].测绘科学,2020,45(05):132-139+151.DOI:10.16251/j.cnki.1009-2307.2020.05.020.

3、算法伪码

import numpy as np


def line_fit_3d_points_m(points):
    """
    全局最小二乘拟合空间直线
    :param points: 三维点集合
     直线方程：
     x = a * z + b
     y = c * z + d
    :return: 直线参数向量数组X
    """

    n = points.shape[0]  # 点的个数
    B = np.zeros((#, 4)  # 矩阵B
    L = np.zeros(#, 1)   # 矩阵L
    X = np.ones(#, 1)    # 解X
    deltX = np.ones(#, 1)   # 迭代增量deltX
    # 构建计算所需矩阵
    for i in range(0, n):
        B[# * i, 0] = points[i].z
        B[# * i, 1] = 1
        B[# * i + 1, 2] = points[i].z
        B[# * i + 1, 3] = 1

        L[# * i] = points[i].x
        L[# * i + 1] = points[i].y
    # 迭代计算
    count = 0
    while norm(deltX) > 0.0001:
        W = B.T*l
        deltX = inv(B.T*B)*W

        X += deltX
        count += 1
        if count >= 10:
            break
    print("迭代次数:", count)

    return X

4、算法效果