SOFT-SLAM阅读总结

本博客是用来记录自己阅读该论文时的随笔若有什么错误希望大家及时指出一起学习

       该SLAM系统设计的主要目的是确保无人机自动导航的计算效率，因此必须要确保该SLAM算法的实时执行和高速可靠性所以该控制系统确保小车可以依赖于在线SLAM进行自主运动。为了实现系统的高速准确率，算法每次迭代几乎都花费恒定的时间是很重要的。为了实现以上要求，我们的算法由两个主要的线程组成：里程计线程和绘图线程，如下图：

Figure1

 

 

里程计线程对于局部可控性方面很重要，绘图线程在全局操作的执行上很重要。这两个线程最终通过一个利用绘图线程的输出以及将该输出与视觉信息融合的滤波器相关联。

两个线程的概述：

　　里程计线程：首先获得了一个新的立体图像对后高质量的特征被检测到，该检测部分是基于非极大值抑制的梯度图中的角点和斑点模糊。其后的IMU是用于预测UAV的相对位移目的是定义一个特征匹配的搜索半径。特征点通过循环匹配，使用图像梯度块的SAD算法加权到被预测的坐标的距离，这一步的输出是一个以后可以使用RANSAC优化的稀疏特征点集。值得注意的是我们系统中有两个不同的RAMSAC优化算法，这两个算法取决于系统的设置，如果一个IMU是可获得的我们直接应用它的陀螺仪测量值来加速并且只执行单点的RANSAC优化。然而如果一个IMU是不可获得的，通常使用三点算法。这种RANSAC的结果是利用高斯牛顿优化设置内点为了得到最终的相对旋转和平移，该过程避免了都在同一时间优化，这样处理的会更快表现也更鲁棒。通过结合获得的相对位移，我们有一个代表基于里程计平移的 作为累计里程计的结果。

绘图线程：得到来自里程计线程的特征点管理模块输出，并将该输出放入帧序列为了以后的关键帧管理步骤。这一步检查了一个当前关键帧是否需要被纳入考虑或者是否要被丢弃，这取决于当前关键帧与上一被存储的关键帧之间的距离，如果该帧被纳入考虑则算法检查回环的可能性。回环检测之后我们在稀疏的位姿集处（特征点不被包括）运行ｇ２ｏ这会导致位姿图更新，因此校正里程计漂移。    绘图线程的输出可以看作由 提出的作为地图到里程计转换的矫正。

这两个线程最终通过一个指数滤波器结合并且结果可以看作提出了一个基于地图的平移 该平移被用于位姿控制器

以上提出的SLAM框架能够保证一个完全分开的里程计部分和绘图部分，这样确保了里程计能够在连续时间里不需要等待绘图进程来运行。这整个方法仍然保留了足够的资源这样在该环境下使用一个双目的方法来计算深度地图也可以构造一个稠密地图，在这里我们特别使用了半全局算法并且我们将结果插入全局ｏｃｔｏｍａｐ，ｏｃｔｏｍｐｓ并不参与位姿估计的过程但在ＥｕＲｏｃ的竞赛中避免冲突是很必要的

两个线程的详细描述：

１．    里程计线程：基于两种情况：（１）在自动UAV的应用中IMU是系统设置的一部分（２）没有传感器信息的基于视觉的ＳＬＡＭ。由于这两种情况我们提出了两种类型的里程计算法：一种使用来自IMU的信息另一种只依赖于视觉

２．    特征点管理

里程计线程开始于一个由三个部分组成的特征点管理：特征提取和匹配，追踪，选择。作为该步的一个输入我们使用一个双目图和一个可获得的IMU测量值

特征点管理开始于提取和匹配那些既在双目对左图和右图的角点，基于这个目的我们利用图像梯度中的角点和团模糊和非极大值抑制，因此得到了有效特征点集。这些特征点随后被用在匹配过程中该过程中的一致性通过计算绝对偏差的和大于一个图像被检测到的最大像素，因为SAD算法容易受到外点的影响，我们格外通过应用环形匹配过滤特征点。通过循环匹配，意味着特征点仅当他们在两个相一致的连续帧在步骤K-1和K….中的左右图都是匹配的才会被考虑，图像由  表示。该循环算法从一帧中找到一个特征点并在另一帧中找到最佳匹配，按照以下顺序进行 ，如果IMU的数据是可以获得的我们预测从步骤k-1到k，否则我们使用恒定速度模型假设并且应用上一个获得的相对位移。最终如果特征点在接下来的序列中匹配成功，上一个匹配特征点与最开始的那一个相同，那么循环关闭并且特征点被考虑为稳定的，因此成为一个高质量的点为以后的分析，否则循环未关闭该特征点被丢弃。此外如果循环成功闭环，我们计算归一化互相关NCC在每一个特征点位姿的矩形区域，该归一化互相关与基于SAD算法一起格外用在环形匹配中。因为NCC明显比SAD算法慢得多，我们只在基于SAD算法的循环匹配执行后运行NCC算法目的是丢弃一定数量的现存的低NCC分数的特征点。

       剩下的特征点集可以通过时间来追踪，并且我们按照以下几个属性来对他们进行联系，包括：i唯一的标识ii阶段iii在图像区域中的当前位姿iv特征点长度v类vi最初的描述子。每个特征点的追踪算法步骤如下：

(1)起始步骤：一个唯一标识符和描述子被添加到附属特征点中并且该特征点的阶段设置为0

(2)追踪：当前帧特征点位姿的每一次迭代在起始帧的特征点位姿方面的一个亚像素阶段被改善见图二 

 

 

通过使用描述子的起始值在接下来特征点的生存期间，漂移被减少因为允许不相似的起始点和当前点保持约束。然而一个视角与首个特征点间紧密的联系可能会导致特殊特征点的追踪失败，另一个选择根据先验位姿估计可能会扭曲起始特征点，然而除了轻微的增加计算复杂度，扭曲也会要求一个patch normal计算。因为该计算可能会成为实时应用的一个开销，一个常用的实践是假定该patch normal是指向相机。虽然该假设会延长特征点的寿命，它也会不可避免的增加一个错误并会随着特征点寿命的增长而加大。通常来说长时间被追踪的特征点被认为更可靠，但也可能在它的生命周期内由于视角的改变导致偏移；因此应该试着保持新的特征点和可靠的特征点之间相同的分配。这可以延长特征点的生命周期通过随着时间特征点的更新，但是另一方面特征点会很快与起始真实世界的点失去一致性并且更易发生漂移。此外一些有着明显特征的特征点甚至会与真实世界缺乏一致性 例如前景线与背景线的交点。由于以上原因我们决定仍然保持描述子的起始值在整个生命周期中，虽然冒着早期特征点死亡的风险但会有一个更加可靠的特征点设置。此外我们传递位姿改善在一个亚像素等级当位于当前事件和之前事件之间建立匹配就像马尔科夫过程一样，也就是说假设关于第一个事件的相对位移的信息是保存在改善的特征点位姿中。我们在实践中使用的是抛物线拟合的方法。追踪过程的具体阐述在图二中

        相比于在以后的过程中估计帧间运动使用一个很大的特征点集我们选择了一个相似但是谨慎挑选的特征点集，我们提出了一个选择策略：提供了在整个图中既有空间特征又有时间特征的特征点，我们尤其选择了能使远特征点和近特征点数量很好平衡的特征点，并且这些特征点被均匀的分布到接下来的图像中这是通过将图像分割成矩形/桶形来实现的。在每一个桶形中仅有一个限制数量的特征点被保留其他剩余的被丢弃。我们的实现表示这样的特征点分布提高了定位的准确性。步骤如下：

1.    图像被卷积成5x5的角点补丁和5x5的斑点补丁，这两个卷积图像的最大值和最小值代表了图像坐标的四类特征点：最大角点值，最小角点值，最大斑点值，最小斑点值。特征描述子由图像水平和垂直梯度周围的像素值构造

(2)图像划分为50x50个像素，区域成为桶，并且每个桶形中的特征点分为四类：最大/最小角点和最大/最小斑点

(3)每一个类中的特征点由以下比较函数存储 

 

(4)选择标准：每一个类别中的第一个特征点被放在最后的列表里该步骤被重复直到选择到要求的特征点数量

     这时我们选择年龄为ath的老特征点，一旦一个特征点太老，该策略选择一个更强健的特征点并且该特征点继续存活仅当他足够强健而它不是因为它的年龄而享有特权。这样我们允许新的强壮的特征点进入选择，防止太老的特征点支配里程计。空间多样性策略首先由桶形实现然后通过在最终的特征点设置中支持不同类别的特征点，而时间变换策略是通过排序选择函数来维持新旧特征点之间的平衡实现。通过遵循以上仔细筛选的逻辑我们确保该里程计是基于一组非均质特征集，该特征点集对于帧间运动估计的准确度是一个很重要的工具

对以上得到的特征点集进行管理分为没有IMU的和有IMU的

2.1没有惯性信息IMU的位姿估计

   没有IMU的里程计 估计图如图三 

 

因为我们依靠单目五点法，该方法不受左右相机间不完美的标定影响，整个帧间运动估计由两步执行：首先旋转使用五点法估计，接下来作为结果的旋转在图像平面经过最小化重投影误差用作位移估计。为了计算时间k-1到k之间的相对旋转我们只使用双目对中的左目相机。一个标定相机的在两个视角中的极线约束可以被表示为    q'TEq=0

矩阵E=[t]xR∈R3x3 是本质矩阵，[t]x是相应的斜对称矩阵而q->q’是在两个视角中的对应的归一化图像坐标。因为单目视图中尺度是不可观测的，八个未知的参数需要被决定。该问题的解决方法首先是选择五个对应项并构造相应的五个方程进行求解，然后使用接下来的本质矩阵的属性 这样一个被定义系统的解位于十次多项式的一个根中。这个五点法在这里被使用通过结合如下的RANSAC算法，从所有的点集中选出五个点的自己用于计算出一个本质矩阵，因此形成相应的假设。每一个获得的本质矩阵被测试所以所有点中内点的数量是确定的，并且假设最大内点的数量被选作最终的结果，我们将它表示为

 

 

 

 在我们的实验中，还测试了一个方法作为额外的改善具体为：将以前获得的最佳五点法的结果进行更新，将一个更大的内点集经过优化得到。然而这会导致平均里程计错误的增加，取决于内点的配置，该方法可能会收敛到局部最小值由原来的五点法解旋转而来。此外原始的五点法有一个封闭式解并且并不依赖于原始推测也不使用最小点集，因此一个外点影响最终结果的可能性被最小化。

         基于滤波的目的我们提出了在上一帧和几个以前的帧之间重复五点RANSAC优化算法，特别的是我们大多数重复该算法三次取决于在步骤K到K-1之间仍存在的特征点的数量。假定K-1和K之间的上一个相对旋转可以表现为如下的不同方式 

上标r代表五点法RANSAC的应用，而 表示以前里程计的相对位移。估计位移 使用了融合球面线性插值的方法(SLERP)，该方法被如下迭代使用 

 

并不是所有的迭代都会被执行，例如如果在步骤K-I中由很少的特征点仍然在步骤K中只有I-1次迭代会被执行，我们通常使用四元数代表旋转，因为SLERP在旋转空间执行，该旋转被用作平移估计部分的输入，在该部分我们使用单点双目法。

        单点双目发依赖于当前帧和以前帧3D点云的重构造，最小化图像重投影误差得到的运动估计比通过在3D空间中获得的点用最小化欧几里得误差更加精确。这是因为位姿不确定的特征点在最小化时不纳入考虑中的事实，此外在欧式空间中误差协方差是高度各项异性的而在图像空间中误差更均匀的分布在维度之间。因此3D点云从以前的视角被三角化然后通过以下公式重投影到当前视图的图像平面上： 

 

对于以前获得的点集中的第i个点，该优化的执行足够快所以它可以在该点集中的每一个点中被执行。因此每个点都可以被看作是被用作测试全部特征点集的假设。选择内点集最大的那一个用作外点抛弃方法，通过这个方法我们获得了最终的特征点集用作全优化的输入。基于这个目的我们应用了全高斯牛顿优化方法，代价函数如下: 

Wi是第i点的权重 

Plens由双目摄像头和镜头设置决定，通过使用该权重函数我们在水平方向上赋予靠近图像中心的特征点更多的含义当减少位于图像水平边缘很远的特征点的影响，以这种方式我们完成了两步间的相对运动由五点法旋转部分所产生的结果，该运动最终基于平移集合成一个完整的里程计

2.2有IMU惯导的位姿估计

如果一个系统设置中有IMU我们可以直接应用它的测量并且避免了使用5点RANSAC方法这就减少了计算的复杂度，算法过程如图4

 

   

     我们将IMU测量值只用于旋转接下来的方法与没有IMU的相似，使用单点优化方法用于平移的估计。

      此外还使用卡尔曼滤波估计陀螺仪的偏斜，一旦一个新的IMU数据被获得卡尔曼滤波器的预测步骤便执行，每当有一个新的图片获得该更新步骤便会被执行 。我们使用该卡尔曼滤波的最后的测量值作为该视觉里程计的输出。

       IMU的测量值与估计的陀螺仪偏斜相结合用作外点抑制，外点可能由动态物体所造成为了缩小特征匹配的搜索范围，减少了执行时间，然后我们执行单点方法为了计算平移矢量，每一个获得的R，t假设都会被测试所以内点的数量在所有点中是确定的，并且再一次最大的内点数量假设被选为最终的抵御杂点，因此最终的特征点集作为全优化的输入。对于全优化我们使用高斯牛顿优化算法为了同时决定旋转和平移

       指数滤波器

      得到了里程计估计 和使用上一个可获得的里程计漂移估计 我们准备更新基于地图的平移 ，然而对于在真实世界中使用UAV的情况如在EuRoC例子中，更新并不是立刻产生的因为位姿快速地转换可能会对小车的稳定性产生消极影响，因此UAV位姿的估计由四元数呈现并且平移矢量对由以下的指数滤波器更新 

 

二.绘图线程

 1. 绘图线程与里程计线程是分开进行的，但它使用特征管理过程的输出序列。绘图线程可以独立执行的主要原因是它的输出作为漂移估计，而漂移假定为维纳过程的一个脉络。因此一旦一个绘图线程中新的输出可获得即一个新的基于里程计的地图矫正 被估计，无论该序列的帧有多老都可以被使用。绘图线程由图二中的几个步骤组成 

它由从序列中取一帧开始，由关键帧的管理来连续根据回环的可能性，并相应的准备位姿用于图优化，图五给出了由soft-slam得到的环境地图 

2.关键帧的管理

          关键帧的管理依赖于由提取步骤得到的特征点集，输入的特征点集看作一个帧，关键帧管理的过程如图六 

每一个帧都看作一个潜在的关键帧通过检查以前关键帧和当前帧之间的距离是否超过了预先设置的阈值在平移 和旋转 之间，如果距离比阈值小我们丢弃当前帧然后用帧序列的下一帧，如果距离大于预先设置的阈值我们检查回环的可能性，如果没有检测到回环将当前帧作为一个新的关键帧插入并且将当前位姿和协方差与它相关联，该协方差被初始化，这样该平移和旋转部分与位于之前关键帧的当前关键帧间的相关因素 成相关性换句话说，我们遵循里程计的逻辑：平移越高，不确定性越高。另一方面如果回环被检测到我们检查新关键帧与闭环关键帧在使用里程计信息的关键帧地图之间的距离，如果距离超过两个阈值  我们将该关键帧插入到完整的关键帧地图中。否则我们将它作为临时关键帧的标志一旦图优化完成它就会从完整的关键帧地图中移除，在继续图优化之前我们要初始化由新的关键帧和其他回环被检测到的关键帧间形成的新的边缘。在以前的协方差初始化脉络中我们使用相关因素 增加了估计里程计信息和那些检测到回环的加入到关键帧的距离，图七图八展示了视觉地图的关键帧和EuRoC数据集轨迹之间的估计联系 

一个临时的关键帧对于回环和图优化是暂时必要的帧，但在长期的情况下它会成为一种开销因为图已经有了一个携带有相似信息的临近关键帧，经过每一个图优化后所有临时关键帧被删除确保在有限的空间下有有限数量的关键帧

3.回环

        回环过程开始于确定候选关键帧是否会被考虑用作回环，基于这个目的我们应用基于新的关键帧位姿的半径和搜索角度，特别只有当关键帧替代了预先定义的半径和角度才会被考虑作为回环候选，因此减少了该步的复杂度。一旦我们有了候选集我们评估他们中的每一个我们比较每一个关键帧和新的关键帧的直方图，该直方图有四个类别组成，最大/最小角点，最大/最小斑点，对于两个直方图之间的距离或者相似度我们使用SAD，在确定了相似信息后我们选择n个关键帧作为回环候选帧，其他剩下的关键帧被丢弃，我们按照以下的过程评估候选集：

 (1)确定两个候选关键帧之间的一致性并且新的关键帧使用循环匹配和NCC

(2)执行三点RANSAC用于确定最适合的平移推测并且应用外点抛弃

(3)如果剩下的内点数量比阈值大我们全面改进我们获得的相关联平移

(4)如果我们发现最终关联距离太大那么我们停止回环

得到了位姿图中的关键帧排列我们使用LevenbergMarquard算法 使用g2o用于优化。为了准备关键帧地图用于图优化除了用于初始化的位姿位姿约束也作为图边缘，g2o也需要边缘协方差矩阵，即不确定的相对位姿约束。基于这个原因我们再次使用一个直接的启发式——不确定的相对旋转和平移分别与相对应的因素 在相对运动的大小上成比例