opengl场景大作业_激光SLAM|SuMa：基于Surfel地图的室外大场景SLAM

论文题目：Efficient Surfel-Based SLAM using 3D Laser Range Data in Urban Environments

开源代码：https://github.com/jbehley/SuMa

作者：任乾

之前我们介绍过SuMa++，它是一个语义SLAM，借助语义提高了匹配精度并移除了动态物体，最终提高了点云地图的质量。而本文介绍的SuMa则为SuMa++提供了基本的SLAM框架，算是它的基础版，它包含了很多细节方面的介绍和实现方法，为了更好地理解这些方法，我们有必要对这篇文章做个介绍。

既然有了增强版，这里就只介绍原理了，效果就不做过多介绍，感兴趣的可以去论文中看

一、前传

这篇文章的核心就是使用Surfel地图去实现前端里程计和闭环检测，因此理解这篇文章的核心就变成了理解Surfel地图。

Surfel地图并不是作者原创，但是该作者第一次把它用在室外大场景三维SLAM中，所以为了摸清Surfel的来龙去脉，我们对它的历史做一个梳理。

Surfel地图最早是用在基于RGB-D相机的三维重建任务中的，在Surfel出现之前，基于RGB-D的三维重建以KinectFusion[1]为主，后来受到[2]的启发，ElasticFusion出现了，这是Surfel地图第一次被完整表述。这种地图表示方式出现之前，三维重建都是存储关键帧，然后以关键帧作为顶点建立概率图然后做图优化，这种属于优化轨迹，而Surfel地图引入之后，优化的元素变成了地图中实际的点(当然不是单纯的三维点，而是以一种特定的组织形式)，以地图一致性作为最终的优化指标，这样不仅提高了地图精度，而且不会像优化轨迹那样因为在同一个地方反复走，虽然地图其实并没有增加，但是优化的概率图却越来越大。这种优点对slam来讲而已同样适用，所以很快就被引入slam领域，[4][5][6]就是典型的例子，但这些都是在室内小场景，我们自然会想到对于室外大场景，它的作用是怎么样的呢？所以就有了本篇论文。

二、算法介绍

1. surfel介绍

既然SuMa其实就是Surfel方法在室外大场景的应用，更具体说是ElasticFusion方法在室外大场景的应用，那么我们就可以按照ElasticFusion的思路先把流程捋一捋，这样就可以更好地理解本文的方法了。

先看下surfel是个啥吧

ElasticFusion的核心流程如下图所示，可以总结为以下几步：

1)ElasticFusion 通过 RGB-D 图像配准计算位姿

2)计算相机位姿如果误差大于设定阈值，表示跟踪失败，启动重定位算法；如果误差小于设定阈值，则进入下一部分。

3)利用 Random Ferns 进行全局的回环检测算法，检测是否存在全局的回环，如果存在全局的回环，计算得到相对位姿变换后，在图像中均匀抽取一些点，建立约束，优化 node 参数。

4)在第 3 部中如果不存在全局的回环，则检测是否存在局部的回环，如果存在局部的回环，则同第 3 步，进行位姿估计，并且建立约束，优化 node 参数。

5)计算得到相机位姿后，将当前帧的点云和重建好的做融合，融合使用 openGL 的 shading language，如果在存在局部的或者全局的回环，在使用 openGL 进行点的融合时候，将优化之后的节点变量，作用于全部的点。

6)融合到全局模型中后，再用 openGL 投影得到当前视角下可以看到的点，用来对下一帧图像配准。

作者用一张图梳理了自己的算法流程

步骤标的明明白白，我们就按照这个来一一介绍

1)处理点云

这个处理就是把点云从三维展开成二维，就是把点云从正前方切开，按照从0到360度的顺序再给他平铺开，从三维到二维的投影可以按下式进行

投影之后，要计算顶点和向量(这两个是surfel地图的元素)，点云、顶点和向量三者的关系如下图所示，其中P就是原始点云，Vd是顶点，Nd是向量。

2)局部地图

局部地图是用来给当前帧做匹配的，把当前帧和局部地图做匹配比跟上一帧做匹配要精确，如果是第一帧，则直接作为局部地图使用

3)位姿更新

把当前帧和局部地图使用ICP方法做匹配，获得位姿之后，再按照当前位姿做投影去更新局部地图

4)更新surfel地图

这个surfel地图就是把历史帧看到的所有信息包含进去，每次有新的关键帧进来，它就要重新对每个surfel单元做初始化，因为实际效果显示，重新初始化surfel地图比在原有surfel基础上对地图做更新要快

5)闭环检测

把当前帧按照位姿投影到surfel地图，如果和时间相隔较远的地图部分有重叠，说明检测到闭环

6)闭环验证

如果和当前帧相似的地图node是时间t添加的，则把t附近的node也找出来，然后把当前帧的node和地图的node建立关联

7)后端优化

如果检测到闭环，则在后端优化的独立线程里去做优化，之后更新地图。

上面这些步骤中，很多在原论文中都有对应的公式，在这里贴公式也没啥意思，大家自己去论文中看吧。

[1]. KinectFusion: Real-Time Dense Surface Mapping and Tracking

[2]. Real-time 3D Reconstruction in Dynamic Scenes using Point-based Fusion

[3]. ElasticFusion: Dense SLAM Without A Pose Graph

[4]. Elastic LiDAR Fusion: Dense Map-Centric Continuous-Time SLAM

[5]. Multi-Resolution Surfel Maps for Effcient Dense 3D Modeling and Tracking

[6]. Online loop closure for real-time interactive 3D scanning