论文题目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