《自动驾驶与机器人中的SLAM技术》之三维空间点云直线拟合

高博的新作《自动驾驶与机器人中的SLAM技术》中提出了一个点云直线拟合的问题，其中的直线方程以及最小二乘目标函数的引出有点突兀（P185-186），这里记录一下理解过程。

1 三维空间中的直线表达方式

（1）平面相交线

对于三维空间中的直线，可以通过两个平面的交线来表达，即：
$$\{\begin{array}{l}{A}_{1}x+B_{1}y+C_{1}z+D_1=0\\ A_{2}x+B_{2}y+C_{2}z+D_2=0\end{array}$$
由上式可知，这种表达方式需要确定两个平面的参数，共计8个，显然不是一个较好的表达方式。

（2）矢量+已知点

根据直线的对称方程有：
$$\frac{x-x_0}{m}=\frac{y-y_0}{n}=\frac{z-z_0}{p}=t$$
式中，$t$是固定比值，通过移项有：
$$\{\begin{array}{l}x=x_0+mt\\ y=y_0+nt\\ z=z_0+pt\end{array}$$
令$P(x_0,y_0,z_0)$表示直线上已知点，$d(m,n,p)$表示直线的单位方向向量，$\bar{x}(x,y,z)$表示直线上的任意一点，则直线的方程可简化为：
$$\bar{x}=dt+P$$

2 三维空间中点到直线的最小二乘目标函数

如上图所示，对于空间中的任意一点${\bar{x}}_k(x,y,z)$，设C直线上已知点$P$到该点的向量，则$C$与直线的方向向量的点乘表示该向量的模长在直线上的投影（对应途中的$b$）,根据勾股定理得任意空间点${\bar{x}}_k(x,y,z)$到直线的距离平方为$({\bar{x}}_k-P{)}^2-(({\bar{x}}_k-P)d{)}^2$,这刚好构成了空间点到空间直线的距离残差平方项，将所有空间点的残差平方项累加便得到了求解空间直线参数的最小二乘目标函数。