回环检测之决策模型

前面我们已经讲了如何描述场景，让机器人尽可能的了解周围环境，那么了解了之后，如何判断出是回环的呢？

本节讨论如何建立决策模型来根据当前场景描述和地图信息识别出可能的闭环.合理的决策模型可有效提高闭环检测的准确度和召回率.这里解释一下准确度和召回率，如下图

其中，假阳性又称为感知偏差，假阴性成为感知变异。我们都希望假阳性和假阴性尽可能少，对于某种特定的算法，我们可以统计他在某个数据集上的TP,FP,FN,TN出现的次数,并统计两个统计量，准确率presicion=TP/(TP+FP),召回率Recall=TP/(TP+FN)。从公式可得，准确率是算法检测到“是回环”的结果里，有多少个真的是回环。（针对结果），召回率是有多少个真的回环，被算法检测到“是回环”了。（针对真实样本），slam中对准确率要求更高，因为如果实际不是回环，算法却判断为回环（即假阳性）会在后端Pose Graph中添加根本错误的边，严重影响算法结果。而召回率低一些关系不大，因为大不了没检测到回环，后端的优化有点漂移和累积误差而已。我们可以通过这两个数据进行评估回环检测算法的优劣。那么怎么判断回环呢？

最简单的建模方法是将闭环检测看作是图像检索问题,而不考虑地图的拓扑信息和度量信息,计算当前图像和地图中所有图像的相似度,找到相似度最大的.如果相似度超过阈值,则认为出现了闭环.这样做的好处是简化了定位的复杂度,提升了定位的效率.对长期大规模 SLAM 来说,地图的信息量会越来越大,过大的搜寻空间会使闭环检测十分耗时.很多采用词袋模型进行闭环检测的系统都使用了图像检索中的一些技术来加速,例如 FAB-MAP 2.0 使用反索引结构 来存储地图描述信息,在每一个词汇下存储拥有该词汇的图片,而并非每一张图片下存储其拥有的词汇,这使得搜寻空间的规模只和词汇数量有关,而不受地图规模的限制.在假设先验信息服从均匀分布的情况下,FAB-MAP 2.0 可看作是一个纯图像检索的过程.虽然只利用图像信息简化了定位的复杂度,但是大量的工作已经表明:利用拓扑信息和度量信息能提高闭环检测的性能 .下面先讨论在只有图像数据和拓扑信息条件下的决策模型建立,再分析将度量信息和几何信息加入到模型中的方法。

利用拓扑信息主要有 2 种方式:一是将拓扑信息融入到概率模型的先验概率中;二是采用序列匹配的方法.

概率模型

闭环检测的任务是根据当前观测信息和地图信息检测机器人是否回到之前到过的地方.从概率的角度出发,就是要生成概率分布,每一个点的概率代表机器人回到地图上某一点的可能性;另外,传感器会受到各种噪声的干扰,因此,大多数闭环检测系统都是通过构建概率模型来进行闭环检测的.

最常用的模型是贝叶斯模型,它主要包含先验模型和观测模型 2 部分. 先验模型的处理相对简单,在没有其他信息的情况下,可假设机器人下一时刻的位置,在上一个时刻位置的某一个邻域内,服从均匀分布或者高斯分布,这一假设实际上包含了地图的拓扑信息.实验结果表明,在大型数据集上这种先验估计可在保证闭环检测准确率的情况下提升 50% 的召回率.

下面讨论观测模型的建立.早期的观测模型都是基于高斯分布的.例如,Vlassis 等用 PCA 生成图像特征,在假设每一个观测的特征都服从单变量高斯分布并且相互独立的情况下,总观测模型由各个特征的边缘概率的乘积表示。

Thrun等假设观测模型服从混合高斯分布并采用 EM(expectationmaximization)方法来估计模型的参数.

Ramos等同样基于混合高斯分布假设,并用变分贝叶斯方法来为每一个场景建立生成模型.

这些方法在实际运用时都存在很大的局限性,主要表现在 2 方面:一是它们都需要一种监督训练的方法来学习观测模型的参数,这使得方法的性能依赖于事先配准的训练数据;二是这些方法都假设地图信息已知而将其作为先验,并假设当前观测一定存在闭环,而实际的闭环检测还需要应对误匹配的问题,这也是闭环检测的难点所在.

Cummins 等提出的 FAB-MAP 算法是一种基于词袋模型和贝叶斯模型的方法,它一定程度上解决了上述方法存在的一些问题,同时获得了一些新特性.这种方法在观测模型的建立上并不依赖于某种特定的分布,而是采用一种数据驱动的方式来计算观测似然.在处理特征关联的问题上,FAB-MAP算法采用 Chow Liu 树 结构代替朴素贝叶斯方法,来逼近视觉词汇之间的依赖关系。

这种方法在处理高维离散分布时,既能节省大量计算条件概率的时间,同时也维持了主要的依赖关系.基于 Chow Liu树的生成模型,使得 FAB-MAP 在进行闭环检测时不仅考虑了 2 幅图像的共享词汇,而且考虑了哪些词汇是一起出现或者消失,这使得即使在共享词汇很少时,FAB-MAP 也能检测出正确的闭环.在处理高频词汇造成的图像混淆问题时,FAB-MAP 采用一种平滑处理的方式来计算概率,这种计算方式加大了先验概率的权重,使高概率的闭环只有通过一系列匹配累加的方式才能产生。但是当图像长时间地描绘非常相似的结构时，其鲁棒性降低。

以上讨论的方法都需要对观测模型进行离线训练.Angeli 等简化了观测模型,没有考虑视觉词汇之间的关联性,而是采用一种增量式在线创建词典的方法,因此不需要进行任何离线训练.为了增强视觉特征的视角不变性,在特征提取的同时计算了 SIFT 特征和局部颜色直方图特征,然后分别生成视觉词典,对当前观测中的每个单词,计算其词频反文献频率(TI-IDF).TI-IDF 值为图片中低频词汇分配更高的权重,而惩罚高频词汇,从而使词袋模型应对图像混淆的能力得到增强.在处理先验概率时,FAB-MAP 只是假设后一时刻的位置是前一时刻位置邻域内的均匀分布, 但是其还考虑了前一时刻概率分布对后续闭环概率的影响。

考虑到局部特征点的描述子在环境变化较大时很难保持不变性,Naseer  等先将图像划分成网格,然后对每个网格计算 HOG(histogram of oriented gradient)描述子.这种密集的图像描述对外观变化的鲁棒性更好.图像间的相似度通过计算描述子间的余弦距离获得,并假设观测模型的概率与相似度成正比.这种处理虽然使整个贝叶斯模型更加简单,增强了外观不变性,但是与词袋模型相比,在处理图像混淆时并不具有优势。因此又有人提出对相似性矩阵进行 ZCA(zero-phase componentanalysis)白化处理,以减少矩阵的模糊性,这种处理有效提高了定位的准确率.

除采用贝叶斯滤波方法外,Burgard等采用扩展卡尔曼滤波来进行闭环检测,但这种方法依赖于特定的 SLAM 系统产生的数据关联,在其他系统上并不具有拓展性.Wolf 等采用蒙特卡洛定位方法,但这种方法依赖于已经建好的地图,并不适用于 SLAM 系统.为解决此问题,Pupilli 等提出了粒子滤波结合无迹卡尔曼滤波的方法,其问题是收敛性受到重采样精度的影响.

序列匹配模型

利用拓扑信息最直观的方法是采用序列匹配.Milford 等的实验结果表明,在场景序列上进行定位相比于单个场景的定位方式,能够有效地减少对数据关联性和场景描述的依赖.这种方式假设机器人在回到同一个地方时,所走的路径与原路径相似,这样闭环检测的问题变为从地图信息中寻找一个局部序列,和当前观测信息的局部序列进行匹配.很多工作表明,这种方法能够在出现较剧烈的环境变化和图像混淆时,提升闭环检测的性能.

Ho 等通过定义相似度函数,用场景的描述信息来计算当前观测序列和地图场景之间的相似度得分,这些相似度得分合在一起构成相似性矩阵.在相似性矩阵的基础上,通过计算累加矩阵来逐步寻找最佳匹配序列.Milford等先用类似的方法构建一个差异性矩阵,在匀速运动的假设下,在差异性矩阵上搜寻一个最佳路径,该路径使得总差异性最小,图 7 展示了这样一个搜寻过程,图中不同斜率的直线代表了不同的速度模型.由于匀速运动假设在实际情况下往往是不成立的,Johns同时考虑了线性和非线性的运动.

除了考虑不同的运动模型来搜寻最佳路径的方法外,Naseer 等将序列匹配的问题表述为最小化网络流的问题.网络流以每 1 组匹配为 1 个节点,节点的边表示位置的转移,网络流的源点和汇点分别代表路径的起点和节点.此外,通过为每 1 个节点引入 1 个隐含节点,使得在计算最佳路径时,可考虑不同大小的速度以及无匹配的情况.Hansen 等使用隐马尔可夫模型进行建模,将当前观测的图像序列定义为可见状态,将对应数据库中的图像序列定义为隐含状态,隐含状态通常可用维特比算法来求解。

序列匹配方法的问题是计算非常耗时,尤其是对于大规模地图. 基于网络流和隐马尔可夫的方法,虽然使序列匹配更加灵活,但是在效率上并没有什么优势.Liu 等通过粒子滤波的方法对候选序列进行采样和更新,在每一轮评价中,高概率序列被保留,低概率序列被剔除,同时新的序列不断加入候选序列中. 这种方法一定程度上加快了计算速度,但是整个时间复杂度仍然为 O(n 2 ),而 且 结 果 的 收 敛 性 依 赖 一 定 的 概 率.

Siam等提出了 FastSeqSLAM(fast se-quence SLAM)算法,其核心思想是它并非将地图上所有序列和当前序列进行匹配,而是在 1 个子集上进行搜索.它采用一种贪婪的搜索方式,先通过 FLANN(fast library for approximate nearest neigh-bors)算法为当前观测序列的每一张图像找到 N 个最相似的图像,然后计算它们之间的相似度得分,这些相似度得分构成一个稀疏差异性矩阵. 此外还利用运动的连续性,估计机器人下一时刻的位置,对下一次的搜索范围进行扩充. 这种方法将SeqSLAM 的时间复杂度由 O(n *n ) 降为 O(nlogn),同时保持了其性能.

以上讨论的方法都是基于位姿间的拓扑关系的,哪些信息被用于计算相似度完全由被匹配的场景所对应的观测信息决定,这种闭环检测机制使其检测效果和机器人路径的离散化产生相关性.Mei等采用了一种基于路标点拓扑结构的闭环检测机制.地图由路标点构成的无向图表示,每 1个路标对应 1 个视觉词汇,图的边表示路标之间的共视关系,这种地图称为共视图.通过将共视图中的团进行聚类形成虚拟的场景,这个虚拟的场景包含着更多共视的路标点,相当于对原来的观测信息进行了增强.这种方法增加了场景定义的灵活性,减小了匹配过程中受路径离散化过程的影响.Cascianelli等同样使用了这种思想,在生成路标时使用 EdgeBoxes 算法提取图像上可能包含物体的候选框,这些候选框作为一种半语义的路标点来构成共视图.在构造虚拟场景时,利用图的代数连通性来对路标点进行聚类,计算相似度时,不仅计算了路标相似度,还考虑了子图结构的相似度.

不同于用概率模型来对闭环检测的问题进行建模,Milford等提出了一种受生物系统启发的模型.这种模型源于鼠类在进行定位时各种脑细胞所呈现的规律,例如代表地点的脑细胞会在老鼠定位在不同场景时出现交替的激活和抑制状态,文章采用一种连续吸引网络(CAN)对这种规律建模.图8 展示了图像序列对机器人方向信息单元的影响,图中 L V 1 等表示图像序列,数字表示不同的角度.不同的图像会使代表不同方向的单元得到不同程度的激活,在图中用不同粗细的箭头表示,只有有序且一致的序列才能使方向的权重分布发生改变,对位置信息的更新同样如此.图像中虚假的或不明确的信息会通过一种吸引子动力学机制来滤除,每一个单元会对其附近的单元产生激活作用而抑制其他单元.闭 环 检 测 问 题 同 样 可 作 为 优 化 问 题 来 求 解,Latif 等注意到了闭环问题的稀疏性,当前观测只和地图中 1 个很小的子集匹配,基于这一点将闭环检测问题转化为 L1 最小化问题,并通过快速凸优化来求解.这种方式使得其在图像像素非常低时也能够成功进行闭环检测.

利用度量信息

度量信息可从 SLAM 系统的前端或者 GPS 中获得,融合度量信息的闭环检测可以缩小闭环搜寻空间,提升闭环检测的能.Maddern 等采用粒子采样的方式来生成可能的闭环,度量信息被融合进粒子的权重中,这样离当前场景距离较近的粒子会获得较大的权重.Bazeille 等则从多个角度利用了度量信息:利用度量信息对观测信息进行等距采样,从而使位置采样独立于运动速度,同时也降低了运算负担;度量信息融入运动模型以生成更准确的先验概率;用度量信息取代极线约束来进行闭环验证. Pepperell 等同样利用里程计信息,在固定距离下对图像序列进行采样,然后再构建差异性矩阵,这种方法使 SeqSLAM 的序列匹配过程具有对速度的不变性.Badino 等利用 GPS 获得地面实况信息,并通过对这些信息进行采样来估计观测模型的概率分布.此外,从度量信息可得到位姿图的拓扑结构,可利用这种拓扑结构对生成的闭环进行验证.Latif 等指出,正确的闭环应该与位姿图的拓扑结构保持一致性,同时正确的闭环之间也应该保持拓扑一致性,基于这一点提出了 RRR(realizing, reversing, and recovering)算法来剔除错误的闭环,以位姿图为基础,通过计算簇间一致性,逐步将正确的闭环添加到图中.

对视觉 SLAM 系统而言,里程计获得的度量信息存在漂移因而依赖正确的闭环进行校正,而闭环检测也依赖于里程计提供正确的先验.研究新的算法,以合理处理二者之间的耦合关系,对视觉SLAM 非常重要.

利用几何信息

对基于局部特征描述子进行闭环检测的方法,可以使用特征点的几何信息来增强鲁棒性. 早期的 SLAM 系统,例如 Konolige等提出的 FrameSLAM,用特征点的 3 维信息采用 3 点 RANSANC(random sample consensus)方法进行闭环检测.这种方法单纯利用特征点的几何信息,因此在面对重复的几何结构时很容易失效.为解决这个问题,

Konolige 引入词袋模型,其基本思想是用词袋模型产生闭环候选帧,然后再使用几何信息滤除错误的闭环.Cummins  等同样使用了这种思想,不同的是对单纯的闭环检测来说,并不需要恢复图像之间的相对运动,而只需检测特征的几何一致性,因而其假设相对运动为纯旋转,以简化模型.以上讨论的方法都使用描述子进行特征匹配,Tardos 等提出了一种基于条件随机场的方法,使其在进行闭环验证时能够灵活地融合各种观测信息.

同时使用特征点的外观信息和几何信息能有效解决图像的混淆问题,剔除错误的闭环,但在人工生成的室内环境和城市环境下存在很多几何结构和外观都相似的情形,在这种情况下还需要结合拓扑信息或者度量信息才能更好地进行闭环检测。