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

文章目录

• 整体流程
• 预处理
• 里程计
• 地图表示
• 相对位姿计算
• 地图更新
• 回环检测
• 候选帧选择
• 回环验证
• 位姿优化
• 代码优化

整体流程

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

• 预处理部分将输入的点云$P$P预处理为点图$\mathcal{N}_{D}$ND​和法线图$\mathcal{N}_{D}$ND​。
• 里程计使用$ICP$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{up}}\right) \mathrm{f}^{-1}\right] \cdot h} \end{array}\right)$(uv​)=(21​[1−arctan(y,x)⋅π−1]⋅w[1−(arcsin(z⋅r−1)+fup​)f−1]⋅h​)
  从点图的临近点计算法线：
$\begin{aligned} \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{aligned}$ND​((u,v))=​(VD​((u+1,v))−VD​((u,v)))×(VD​((u,v+1))−VD​((u,v)))​

里程计

地图表示

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

相对位姿计算

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

地图更新

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

回环检测

候选帧选择

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

回环验证

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

位姿优化

  使用gstm对位姿进行优化。

代码优化

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