【论文阅读】SOFT-SLAM

论文：《SOFT-SLAM:Computationally efficient stereo visual simultaneous localization and mapping for autonomous unmanned aerial vehicles》( JFR 2017 )

摘要

无人驾驶飞行器(UAVs)在GPS缺失环境下的自主导航是一个具有挑战性的问题，特别是对于小型无人机，其特点是载荷小，电池容量有限。针对上述问题，可能的解决方案是基于视觉的同步定位和映射(SLAM)，因为相机的尺寸，低重量，可用性和大的信息带宽规避了所有UAVs的限制。在这篇论文中，我们提出了一种双目视觉SLAM，可以获得非常精确的定位和密集的环境地图，旨在参加欧洲机器人挑战赛(EuRoC)，目标是小规模的UAVs工业设施的空中检查。所提出的方法包括一种新的立体里程计算法，依赖于特征跟踪(SOFT)，目前在KITTI数据集的所有立体方法中排名第一。基于SOFT进行姿态估计，建立了基于特征的姿态图SLAM算法，并将其命名为SOFT-SLAM。SOFT-SLAM具有完全分离的里程计和映射线程，支持大闭环和全局一致性。它还实现了20hz的恒定时间执行率和确定性的结果，只使用在挑战中使用的机载计算机的两个线程。运行我们的SLAM算法的UAV在EuRoC Challenge 3, Stage IIa-Benchmarking, Task 2中获得了最高的本地化得分。此外，我们还在两个流行的公共数据集上对SOFTSLAM进行了详尽的评估，并将其与其他最先进的方法进行了比较;即ORB-SLAM2和LSD-SLAM。结果表明，SOFT-SLAM对大部分数据集序列具有较好的定位精度，同时具有较低的运行时间。

贡献

• 不使用BA(BA太耗算力),使用SOFT里程计进行定位，达到了0.8%的误差和20hz的速度，目前在KITTI榜单上排名第一。
• 提出的SLAM框架支持大范围的闭环检测，并且完全解偶里程计和建图线程(即里程计可以实时运行，不需要等待建图线程的消息)。并且和ORB-SLAM2不一样的是，因为ORB-SLAM2是多线程，所以它会对同样的输入数据产生不同的输出结果，但是这个方法可以保证对同样的输入数据，输出结果相同。
• 使用SOFT特征进行回环，使得整个系统更加高效简洁，并且可以实现亚像素级别的精度，尽管SOFT特征对于旋转和尺度不是不变的，但是作者通过实验证明了在大部分数据集上的准确性。
• 在Euroc和KITTI数据集上进行了系统的分析。

系统框架

里程计线程和建图线程是两个相互独立的线程，这两个线程的输出通过一个指数滤波进行关联。
在里程计模块，用角点和blob mask进行特征提取；IMU预测相对位移，用来确定特征匹配的搜索半径；通过SAD以循环顺序进行匹配，输出一个稀疏特征集。系统包含两种RANSAC方法：如果有IMU，就可以直接利用陀螺仪测量值，采用1点RANSAC，如果没有IMU，那就用5点RANSAC。通过外点剔除后，得到一系列特征点，进行Gauss-Newton优化，得到相对旋转和平移；积分得到基于里程计的变换Tob。
建图线程使用里程计线程计算的特征点集，并选取关键帧，进行回环检测、位姿图优化，来修正里程计偏移，得到变换Tmo。两线程通过指数滤波器融合，最终输出Tmb。

里程计线程

特征点管理

特征点管理分为三步：特征点的提取和匹配、特征点跟踪、特征点选择。双目图像和IMU测量值作为输入。

• 提取和匹配
  提取梯度图的斑点、角点，并做非极大值抑制，得到一些可用的特征点。
  匹配遵循环形匹配规则，使用SAD算法进行。IMU可用时作为预测值，IMU不可用就假设速度恒定。只有在最后匹配的特征点与开始的相同，才留下该特征点。匹配后如果仍需淘汰一部分特征点，则选择NCC低的的特征点淘汰。
  
• 跟踪
  特征点的标签有：唯一标识符；年龄（被跟踪次数）；点的像素坐标；特征强度；类；初始描述符。对于新加入的特征点，age设为0，然后通过跟踪优化特征点的位置。
  
  蓝点和橙点分别是两个不同的特征点，实线代表特征点匹配，虚线代表优化过程，右上角的patch里的灰度等级代表给定的像素的权重更大，最后一行是图片匹配实例。为了加长特征点的生命周期(如果对应特征点变化的过快，则生命周期会缩短，特征点生命周期越长说明这个特征点越有效)，需要根据先验位姿预测来矫正特征点，并且为了降低计算量，一个通用的方法是假设一个指向相机的法向量补丁。这样的假设虽然延长了特征点的寿命，同样也增大了累积误差，所以需要维持新旧特征点合适的比值。
• 选择
  提出了一种选择策略，该策略考虑了跨图像的强特征的空间和时间变化。这个策略使得远近特征的数量很好地平衡，并且特征在整个图像上均匀分布。

1. 首先，图片通过5x5的corner patch和5x5的blob patch进行卷积，两个卷积图像的最大最小值代表四类特征的图像坐标: corner max, corner min, blob max, blob min.然后根据图像梯度中的周围像素值构建特征描述，包括水平和垂直方向。
2. 然后，将图像分成50x50个bucket,每个bucket里有四类特征: corner max, corner min, blob max, blob min。
3. 对于每一类特征，特征点使用如下的对比函数进行排序:
   
4. 按照顺序，对每类的第一个特征点放入最终集合，直到到达一定数量。

无IMU的位姿估计

首先利用五点法的对极约束，计算旋转，然后通过最小化重投影误差计算平移。

每次依次从特征点集中选出五个点，计算一个基础矩阵E，对每个E进行测试，选可以包含最多内点的E作为最终结果。
为了结果平滑，在最后一帧和前几帧前重复五点RANSAC，考虑存活特征点的个数，最多重复三次。最终k-1和k帧图像间的相对旋转可以表达为：

利用SLERP进行迭代插值：（这里实际是对四元数进行计算，而不是旋转矩阵）

最小化重投影误差求得平移：

权重因子由特征点的age决定

有IMU的位姿估计

利用IMU测量值估计旋转，然后与无IMU的位姿估计一样去估计平移。

首先卡尔曼滤波更新陀螺仪偏置，然后偏置与IMU测量值一起对特征点集进行外点剔除，得到旋转矩阵，其他步骤与无IMU相同。

指数滤波

因为真实环境下无人机的突然震荡可能会对位姿更新产生负面影响，所以将位姿分割成四元数和平移向量，然后分别通过指数滤波进行更新：

建图线程

建图线程使用了里程计线程的特征点集，判断每帧输入图像的距离，小于阈值则舍弃，大于阈值进行回环检测：无回环则加入关键帧；有回环则计算新帧和地图中最近关键帧间的距离，大于阈值则插入关键帧，小于阈值则作为临时关键帧，在图优化后，删除临时关键帧。最终图优化结束，输出Tmo。