最小二乘拟合空间直线

本文出自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)将空间直线分别垂直投影到$XOY$、$YOZ$和$XOZ$ 坐标平面，将空间直线的６个几何参数转化为４个几何参数。
  基于最小二乘准则的三维空间直线拟合方法如下所述：
  假设$\widehat{x_i}=a\widehat{z_i}+b$表示按式(4)求得的近似值，则与实测值$x_i$的较差和为：
$$Q_x=\sum _{i=1}^m[x_i-(a{z}_i+b){]}^2\text{\hspace{1em}(5)}$$

  假设$\widehat{y_i}=c\widehat{z_i}+d$表示按式(4)求得的近似值，则与实测值$y_i$的较差和为：
$$Q_y=\sum _{i=1}^m[y_i-(c{z}_i+d){]}^2\text{\hspace{1em}(6)}$$
  基于最小二乘准则，为获得$Q_x$和$Q_y$的最小值，对式(7)和式(8)求取关于$a、b、c、d$的导数并令其为0，可得：
$$\{\begin{array}{l}bN+a{\sum }_{i=1}^m{z}_i={\sum }_{i=1}^m{x}_i\\ b{\sum }_{i=1}^m{z}_i+a{\sum }_{i=1}^m{z}_i^2={\sum }_{i=1}^m{x}_i{z}_i\\ dN+c{\sum }_{i=1}^m{z}_i={\sum }_{i=1}^m{y}_i\\ d{\sum }_{i=1}^m{z}_i+c{\sum }_{i=1}^m{z}_i^2={\sum }_{i=1}^m{y}_i{z}_i\end{array}\text{\hspace{1em}(7)}$$
令：
$$M=\left[\begin{array}{cccc}{z}_1& z_2& \cdots & z_n\\ 1& 1& \cdots & 1\end{array}\right]\text{\hspace{1em}(8)}$$

则式(7)可简化为：
$$M{M}^{T}A=MX,M{M}^{T}B=MY\text{\hspace{1em}(9)}$$
式中：$A=[a,b{]}^T;X=[x_1,...,x_m{]}^T;B=[c,d{]}^T;Y=[y_1,...,y_m{]}^T$。

或者最小二乘直接求解：

$$a=\frac{n{\sum }_{i=1}^n{x}_i{z}_i-{\sum }_{i=1}^n{x}_i\ast {\sum }_{i=1}^n{z}_i}{n{\sum }_{i=1}^n{z}_i^2-{\sum }_{i=1}^n{z}_i\ast {\sum }_{i=1}^n{z}_i}$$

$$b=\frac{{\sum }_{i=1}^n{x}_i-a{\sum }_{i=1}^n{z}_i}{n}$$

$$c=\frac{n{\sum }_{i=1}^n{y}_i{z}_i-{\sum }_{i=1}^n{y}_i\ast {\sum }_{i=1}^n{z}_i}{n{\sum }_{i=1}^n{z}_i^2-{\sum }_{i=1}^n{z}_i\ast {\sum }_{i=1}^n{z}_i}$$

$$d=\frac{{\sum }_{i=1}^n{y}_i-c{\sum }_{i=1}^n{z}_i}{n}$$

2、参考文献

[1] 包建强,张献州,李圆等.多种空间直线拟合方法应用分析[J].测绘科学,2020,45(05):132-139+151.DOI:10.16251/j.cnki.1009-2307.2020.05.020.
[2] 薛丽红.三维空间点中基于最小二乘法的分段直线拟合方法[J].齐齐哈尔大学学报(自然科学版),2015,31(04):84-85+89.
[3] 三维空间点的直线方程拟合(最小二乘法)
[4] 最小二乘法拟合空间直线的原理及实现（这篇博客的方法存在不足！！！）

3、示例代码

import numpy as np


def line_fit_3d_points(points):
    """
    最小二乘拟合空间直线
    :param points: 三维点集合
     直线方程：
     x = a * z + b
     y = c * z + d
    :return: 直线参数a,b,c,d
    """
    x = points[:, 0]
    y = points[:, 1]
    z = points[:, 2]
    n = points.shape[0]
    a = (n * sum(x * z) - sum(x) * sum(z)) / (n * sum(z * z) - sum(z) * sum(z))
    b = (sum(x) - a * sum(z)) / n
    c = (n * sum(y * z) - sum(y) * sum(z)) / (n * sum(z * z) - sum(z) * sum(z))
    d = (sum(y) - c * sum(z)) / n

    return a, b, c, d

4、算法效果