回环检测
一、概述
在此先放一张原著第二章中的视觉SLAM结构框图
图1:经典的SLAM结构框图
从图中可以看出,视觉SLAM系统主要由传感器、前端、后端、建图和回环检测五大部分组成。传感器的作用是信息读取,对视觉SLAM而言就是相机成像过程,在ch5中已介绍。前端的作用是根据两帧图片信息估计相机位并同时实现路标定位的过程。这个过程又称为里程计,在ch7和ch8中已介绍。后端的作用是采用全局优化的方法对相机位和路标进行精调,进一步提高SLMA精度。这种优化方法在ch6,ch9,ch10中已介绍。建图是指根据前面SLAM过程获得的路标,进一步得到能满足现实应用需要的地图,将在ch12中介绍。回环检测是指相机位姿经过一番变化回到原位后,系统感知此信息的过程,是这一篇要讨论的内容。
我们可以参考图2来说明回环检测的概念和意义。图2(a)为真实轨迹,可以看出相机位在经过一个回环后重新经过了A点。图2(b)中的红线为SLAM系统构建的轨迹,可以看出构建轨迹发生了漂移,相机位离真实轨迹越来越远。图2(c)中的绿线为利用回环检测技术构建的轨迹,可以看出它和真实轨迹吻合的比较好,说明回环检测的必要性。
图2:漂移示意图。(a)真实轨迹;(b)优化后轨迹;(c)有回环检测轨迹
在没有后端的情况下漂移产生的原因是容易理解的。这是因为前端只根据两帧信息来确定它们之间的相机位姿差。基于这种信息构建相机轨迹的过程和基于惯导器件数据积分得到位置的过程很相似,因此难免产生漂移和误差。如果系统带后端BA过程,则漂移量会大大降低。为了减少计算量,BA一般不能全局开展,而只能在滑窗中进行。这种情况下即便带后端的系统也会产生漂移。
回环检测的目的,就是让系统能感知自己走过的位置,如图2(a)中的A点。这样利用这两个位姿下的共同路标点就可以有效校正相机位的漂移。现在的问题是,如何判断相机回环或回到了原位A呢?第一种思路是根据里程计的几何关系来确定,如图2(b)中红线的交点位置所示。显然这种方法所得到的回环位置与真实的回环位置A会存在一定的误差,只能在累积误差较小的情况下使用。第二种思路是根据外观的几何关系,即在SLAM过程中不断检测新相片与历史相片是否相似来检测回环关系。这种思路无疑更严谨,但困难在于如何判断两张图片相似。最简单的想法是直接将两张图片的灰度相减并根据结果的范数来判断。这种判断方法在实际应用中会存在问题。当相机位发生细微的偏差时,这种方法所得的范数都可能很大,这是因为两张图片相减时特征点没有对齐的缘故。如果要针对每帧数据都提取特征点并与历史相片比较,以特征点匹配数量足够多来判断回环,则实际运行会发现计算量很大。目前实际采用的是基于词袋模型的回环检测方法。
二、词袋模型
词袋,也就是Bag-of-Words(BoW),目的是用图像上有哪几种特征来描述、区分和匹配一幅图像。例如,我们说某张图片中有一个人,一辆车;而另一张图片中有两个人,一只狗。根据这样的描述,就可以度量图片的相似性。具体说,我们要做如下三步:
(1)确定“人”,“车”,“狗”等概念。这对应着BoW中的单词(Word),许多单词放在一起,组成了字典(Dictionary)。
(2)确定一幅图片中出现了哪些字典中的词及其频次,这样可以用一个向量描述一张图片。这个向量的基就是词袋中单词的某种指定排列。
(3)利用两幅图片所对应向量差的某种范数来表征两张图的相似性,范数越小就越相似。
三、字典
我们前面用“人”,“车”,“狗”等概念表示一张图片中的特征。在实际应用中我们不可能使用这些概念,这是因为我们目前暂时无法用计算机描绘这些特征的缘故。一种可行的方法采用一组特征点的组合来表示单词。特征点组合可采用聚类的办法来产生。如图3所示,在图片中取k个中心点,然后计算所有特征点与中心的距离,取距离最小的中心为特征点的归类。这样所有的特征点就会聚集到中心各种中心周围,每个中心及其周围的特征点构成一个聚类。这些聚类就可以用作字典中的单词。
图3:K-means聚类的概念----图片引自云天徽上的CSDN博客
在实际应用中,先对图片的特征点进行聚类以得到图片中所包含的单词,然后根据这些单词来查询其在字典中的位置,就可以确定表征这张图的向量。有了这个向量,就可以比较两张图片是否相似并判断回环。
在具体查询字典时,我们还需要用到数据结构的概念。这是容易理解的,我们在查新华字典的时候并不是从字典中的第一个字开始比较并遍历整个字典。以拼音检索为例,我们会先根据声母查找到一定的页数,然后根据韵母查找再翻几页,最后根据声调来定位。类似地,聚类或单词的查找也会利用这种数据结构以加速查询过程。首先我们需要构造聚类库的数据结构。其具体做法就是先做大聚类,然后对将每个聚类分为小聚类,以此类推一直到足够小的聚类为止。最小的聚类即树结构的叶子就是单词,而大的聚类或树干只是用于查询方便,本身并不是单词。字典的生成以及查询都有现成的BoW,要深入理解这方面的内容需要结合实际库和应用代码来进行。这将在后续的程序分析系列阐述。
四、相似度计算
有了字典以后,给定任意图像特征(特征点的聚类),就可以按数据结构查询对应的单词。如果能查到,就相当于图像拥有某种特征,例如这幅图里面有一个人和一辆汽车。这种方法对所有的特征一视同仁,在实际应用中没有考虑单词的区分性。好比说,“的”“是”这样的常见字可能在很多句子里面中出现。我们如果根据是否含有这样的字来判断两个句子相似就会产生很大的误差。反之,“文档”“足球”这样的单词,对于判断句子相似性的作用就大一些。既然如此,如何让聚类区分是否常见字词呢?
首先我们在构建字典的时候,针对每一片叶子即单词同时记录它在构建过程中出现的频次以及所有叶子出现的总频次。定义一个参数IDF(Inverse Document Frequency)为
(1)
这个参数表示单词在字典中出现频率的倒数(求对数只是为了减少它的数值)。值越大,表示它越少出现,相当于“文档”“足球”之类的单词,而不是“的”“是”之类的单词。
第二步,我们针对某一张图片统计特征出现的频率。假设图像A中单词出现了次而一共出现的单词次数为,那么定义一个参数
(2)
这个参数表示特征出现的次数。这样定义为单词的权重,从而得到图片A的一个向量
(3)
下面一行表示向量的基矢量排序。为了比较两幅图片是否一致,只需要计算它们的某种范数的值即可。如果这个值很大就表明这两张图的差异大,反之差异小。
五、小结
这一节讨论回环检测的目的和方法,阐述了基于词袋模型的图像相似性判断思想。要详细理解其中内容必须与程序相匹配,将在后续的系列博客中进行。这个系列的最主要目的是为了让读者快速入门,对SLMA的主要过程和方法有一个基本的概念。因此阐述过程中略去了很多细节,例如原著中关于的计算公式(11.9)存在笔误等。正如高博在原著14章课后最后一道习题中所说,“你应该能看懂绝大部分的SLAM相关论文了,赶紧开始你的研究吧”。本系列的目的正在于此:让读者快速入门,开启自己的SLAM研究进程。