Elastic LiDAR Fusion Dense Map-Centric Continuous-Time SLAM 概述
摘要
时间连续轨迹表达的提出为现代多传感器融合SLAM提供了更高的精度和效率。然而除了这些优势，一些由于全局优化的需求造成的一些问题阻碍了这类系统的实时和长周期的应用。文章提出了一种基于连续时间轨迹的稠密的以地图为主的SLAM方法来解决以上问题。提出的系统和传统连续时间轨迹SLAM（CT-SLAM）方法类似，不同点在于通过使用全局一致的地图而不是对于全局轨迹的优化。该方法在闭环检测的时间复杂度上不依赖于操作时间，依赖于闭环检测前它已经探索的空间。所以比传统的CT-SLAM更加适合于长周期的操作。另外，这种方法使用surfel模型表示地图所以减少了重建稠密地图的不确定性。文章使用全局一致地图而不是进行全局轨迹优化而且通过使用surfel融合有效减少了LiDAR的噪声。
主要贡献
1.提出了一种新的多传感器SLAM系统
2.使用全局一致的地图代替了全局轨迹优化
3.使用surfel减少了激光点云的噪声
算法流程
图1.（a）手持3D旋转LiDAR做移动测图。包括一个2D的激光，一个IMU，一个编码器，一个彩色相机还有一个热成像相机。热成像相机在文章中并没有使用。（b）文章SLAM系统流程图。Local轨迹计算是在Local Mapping步骤进行，Global Mapping模块生成全局一致的地图。
文章的方法分为两个模块，Local Mapping和Global Mapping.
Local Mapping模块中使用了IMU和LiDAR通过连续时间轨迹优化的方法建立了校正过运动变形的地图。这一步与CT-SLAM中滑动窗口的操作比较类似，但是主要的区别在于CT-SLAM中利用稀疏的全局地图作为定位的先验。闭环检测模块在移动窗口中不断地产生关键点然后和之前产生的关键点作为比较。
Global Mapping模块中建立和融合了surfel地图。文章两种不同的surfel地图作为稀疏和稠密的地图起到了不同的作用。稀疏地图multi-resolutional 3D ellipsoidal surfel地图是为了快速鲁棒的进行连续时间轨迹的优化，对于稠密建图来说过于稀疏。因此，2D disk surfel地图被用来做稠密的融合。
全局地图的一致性是利用闭关检测约束来做非刚性的形变达成的。这部分的方法受到ElasticFusion的启发。文章通过考虑到surfel不确定性的传播对其进行了扩展。除了闭环检测约束，通过图来表达这种非刚性的形变来减少闭环检测约束的误差。在优化这个图之后，这种变形被应用与全局的地图。
Local Mapping
- 连续时间轨迹的表示
任一时刻的变换矩阵可通过对相邻的轨迹要素进行内插计算，定义为：
其中R,t通过timestamp相邻的两个时刻k,k+1的位姿进行内插给定：
其中w为相对运动，定义为
内插系数为
目标函数fn以传感器观测值zn和位姿T为输入，位姿T是用轨迹元素Q进行内插得到的：
L为第n个约束的第l个观测值。该方程可以用迭代的非线性最小二乘来解。
T是轨迹元素进行内插确定的，所以在优化时，更高频的系统观测值能增加雅可比矩阵的行数而不是状态的维度，这有别于一些传统的固定时间的轨迹模型。
B．Local Tajectroy的优化
图2. Local trajectory的几何约束。在两个内插的位姿点Ta和Tb可以生成两个surfel模型ua和ub。Local sweep uc和map的先验um之间的几何约束会使整个轨迹适用于地图的先验信息。
这一步先通过对点云点生成稀疏的ellipsoidal surfel。在两个内插的位姿点Ta和Tb可以生成两个surfel模型ua和ub。Local sweep uc和map的先验um之间的几何约束会使整个轨迹适用于地图的先验信息。由此我们可以计算两个相对应的surfel，ua和ub间的距离误差以及uc和先验um间的距离误差，如图2.
其中u为surfel的中心，t和R都是相应时刻内插得到的位姿信息，um是出于世界坐标系下的地图先验信息，不会被改变，所有的移动造成的误差会用地图先验信息来改正。
另一方面，IMU也可以用来辅助校正相对位姿
其中，a和上加横线的w为IMU的加速度和角速度信息，g为重力加速度，R，t，w为内插得到的旋转，平移和角速度信息，两个b为IMU的biases。
最后最小化得到的所有误差来校正轨迹
其中
之后再对生成的稀疏的和稠密的surfel进行融合。
图3. (a)包含闭环的地图，红色实线代表最新的地图元素。(b)闭环的融合过程。上面是融合前，下面为融合后。(c) 在激活窗口内进行融合（绿色实线）。（d）激活和未激活区域的错误匹配的检测。
根据timestamp可以选取激活区域和非激活区域，在其中可以利用ICP来检测重叠或误匹配区域。融合算法如下：
 |
Global map building
文章提出的为了保持地图一致性的方法
图的构建：node为随机选取的surfel的中心。
图的应用：假设node位置为g，旋转与平移关系为R,t,node影响某点pi的方程可以被定义为
同时该点也会受到附近的node,N(pi),的影响，被改变后的pi点定义为
W(p)由node到点pi的距离确定
Dmax为所有待选node到pi的最远距离
新的surfel的法线方向n被定义为
图的优化
这部分主要是图优化的约束
第一个约束是原始surfel psrc和目标surfel pdest
为了防止原始数据和目标数据无限循环加入阻塞约束
为了保证整体匹配光滑，加入以下约束来将误差分配给附近的node。
最后，通过最小化变形误差和闭环误差来优化node转换关系：
只用高斯牛顿法进行非线性迭代。
主要结果
文章方法与CT-SLAM的比较
文章方法与CT-SLAM计算的轨迹和图
文章方法与CT-SLAM的轨迹估计结果比较
文章与CT-SLAM比较使用的数据集
表面估计结果的统计值
文章方法和CT-SLAM结果比较