Efficient Surfel-Based SLAM using 3D Laser Range Data in Urban Environments

整体流程

  如上图所示，SuMa的整体流程可以分为预处理，里程计和回环检测。

• 预处理部分将输入的点云$P$预处理为点图${\mathcal{N}}_D$和法线图${\mathcal{N}}_D$。
• 里程计使用$ICP$算法，根据活跃地图渲染得到的点图，法线图和当前帧的点图和法线图，得到当前帧的位姿，并对地图进行更新。
• 回环检测得到位姿的方法和里程计相同，唯一的区别是使用了不活跃地图渲染得到点图和法线图。得到位姿之后，对回环进行验证，验证通过后进行全局位姿优化。

预处理

  将点云根据水平面和竖直的角度投影成点图：
$$\left(\begin{array}{l}u\\ v\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}\left[1-\arctan (y,x)\cdot {\pi }^{-1}\right]\cdot w\\ \left[1-\left(\arcsin \left(z\cdot r^{-1}\right)+{\mathrm{f}}_{\mathrm{u}\mathrm{p}}\right){\mathrm{f}}^{-1}\right]\cdot h\end{array}\right)$$
  从点图的临近点计算法线：
$$\begin{array}{rl}{\mathcal{N}}_D((u,v))=& \left({\mathcal{V}}_D((u+1,v))-{\mathcal{V}}_D((u,v))\right)\\ & \times \left({\mathcal{V}}_D((u,v+1))-{\mathcal{V}}_D((u,v))\right)\end{array}$$

里程计

地图表示

  本节介绍了基于面元的地图表示和更新策略。每个面元的参数包括三维坐标${\mathbf{v}}_s$，法向量${\mathbf{n}}_s$，半径$r_s$，创建时间$t_c$，最后更新时间$t_u$和置信度$l_s$。
  对于每一次观测，根据观测的角度和距离对现有的面元的置信度进行更新。根据创建时间和最新更改的时间，将点云地图分为活跃地图和不活跃地图。在进行全局地图更新时，只根据时间来跟新每个时间创建的面元的参数，而不需要对地图进行重新构建。

相对位姿计算

  采用速度不变假设作为ICP算法的初值，采用LM算法对相邻帧相对位姿进行求解。

地图更新

  地图更新包括面元的创建和融合，首先计算面元的半径：
$$r_s=\frac{\sqrt{2}{\Vert {\mathbf{v}}_s\Vert }_2\cdot p}{\mathrm{clamp}\left(-{\mathbf{v}}_s^{\top }{\mathbf{n}}_s\cdot {\Vert {\mathbf{v}}_s\Vert }_2^{-1},0.5,1.0\right)}$$
  其中，$p=\max \left(w\cdot f_{\text{horiz }}^{-1},h\cdot f_{\text{vert }}^{-1}\right)$表示每个像素的尺寸，$\mathrm{clamp}(x,u,l)=\min (u,\max (x,l))$，但这个算子很奇怪，难道这个算子的结果不是恒为0.5吗？下文说本文考虑了室外场景面元尺寸不能太大的问题。
  通过匹配渲染图的坐标，得到当前帧面元和地图面元的匹配，如果角度和距离的差值都小于阈值，对地图中的面元进行融合，更新，我觉得下面的式子少了对最后更新时间和置信度的调整；
$$\begin{array}{rl}{\mathbf{v}}_{s^{\prime }}^{(t)}& =(1-\gamma )\cdot {\mathbf{v}}_s+\gamma \cdot {\mathbf{v}}_{s^{\prime }}^{(t-1)}\\ {\mathbf{n}}_{s^{\prime }}^{(t)}& =(1-\gamma )\cdot {\mathbf{n}}_s+\gamma \cdot {\mathbf{n}}_{s^{\prime }}^{(t-1)}\\ r_{s^{\prime }}^{(t)}& =r_s\end{array}$$
  如果差值，超过阈值，创建新的面元，并减少新面元的置信度。在地图更新结束后，将置信度小于阈值，同时更新时间较早的面元去除。

回环检测

候选帧选择

  从不活跃地图中，找到距离最近的帧作为候选帧。为了避免两帧重叠部分较少，导致ICP得不到正确的位姿变换。所以从不活跃地图渲染点图和法线图。

回环验证

  连续几帧匹配验证通过的点认为是可靠的回环帧。

位姿优化

  使用gstm对位姿进行优化。

代码优化

  使用OpenGl的着色器对渲染，ICP等过程进行加速，由于建图规模较大，还需要在GPU和CPU之间进行参数传递。为了避免计算量出现过大的峰值，分布完成上传和下载。