SLAM大法之回环检测

　　随着路径的不断延伸，机器人在建图过程中会存在一些累计误差，除了利用局部优化、全局优化等来调整之外，还可以利用回环检测来优化位姿。

　　什么是回环检测？

　　回环检测，又称闭环检测，是指机器人识别曾到达某场景，使得地图闭环的能力。说的简单点，就是机器人在左转一下，右转一下建图的时候能意识到某个地方是“我”曾经来过的，然后把此刻生成的地图与刚刚生成的地图做匹配。

　　回环检测成功

　　回环检测之所以能成为一个难点，是因为：如果回环检测成功，可以显著地减小累积误差，帮助机器人更精准、快速的进行避障导航工作。而错误的检测结果可能使地图变得很糟糕。因此，回环检测在大面积、大场景地图构建上是非常有必要的 。

　　回环检测失败

       如何提升机器人回环检测能力？

　　那么，怎么才能让机器人的回环检测能力得到一个质的提升呢？首先要有一个算法上的优化。

　　1. 基于图优化的SLAM算法

　　基于图优化的SLAM 3.0 算是提升机器人回环检测能力的一大突破。

　　SLAM 3.0采用图优化的方式进行建图，进行了图片集成与优化处理，当机器人运动到已经探索过的原环境时，SLAM 3.0可依赖内部的拓扑图进行主动式的闭环检测。当发现了新的闭环信息后，SLAM 3.0使用Bundle Adjuestment(BA)等算法对原先的位姿拓扑地图进行修正（即进行图优化），从而能有效的进行闭环后地图的修正，实现更加可靠的环境建图。

　　SLAM 3.0闭环检测

　　SLAM 3.0环路闭合逻辑：先小闭环，后大闭环 ；选择特征丰富的点作为闭环点；多走重合之路，完善闭环细节。即使在超大场景下建图，也不慌。

　　超大场景下建图完整闭合过程

　　2.词袋模型

　　除了SLAM算法的升级和优化之外，现在还有很多系统采用成熟的词袋模型方法来帮助机器人完成闭环，说的简单点就是把帧与帧之间进行特征比配。

　　1、从每幅图像中提取特征点和特征描述，特征描述一般是一个多维向量，因此可以计算两个特征描述之间的距离；

　　2、将这些特征描述进行聚类（比如k－means），类别的个数就是词典的单词数，比如1000；也可以用Beyes、SVM等；

　　3、将这些词典组织成树的形式，方便搜索。

　　
利用这个树，就可以将时间复杂度降低到对数级别，大大加速了特征匹配。

　　3.相似度计算

　　这种做法是从外观上根据两幅图像的相似性确定回环检测关系，那么，如何确定两个地图之间的相关性呢？

　　比如对于图像A和图像B，我们要计算它们之间的相似性评分：s(A,B)。如果单单用两幅图像相减然后取范数，即为： s(A,B)=||AB||s(A,B)=||AB||。但是由于一幅图像在不同角度或者不同光线下其结果会相差很多，所以不使用这个函数。而是使用相似度计算公式。

　　这里，我们提供一种方法叫TF-IDF。

　　TF的意思是：某特征在一幅图像中经常出现，它的区分度就越高。另一方面，IDF的思想是，某特征在字典中出现的频率越低，则分类图像时的区分度越高。

　　对于IDF部分，假设所有特征数量为n，某个节点的Wi所含的数量特征为Ni，那么该单词的IDF为：

　　TF是指某个特征在单副图像中出现的频率。假设图像A中单词Wi出现了N次，而一共出现的单词次数是n，那么TF为：

　　于是Wi的权重等于TF乘IDF之积，即：

　　考虑权重以后，对于某副图像，我们可以得到许多个单词，得到BOW：

　　（A表示某幅地图）

　　如何计算俩副图像相似度，这里使用了L1范数形式：

　　4.深度学习及其他

　　除了上面的几种方式之外，回环检测也可以建成一个模型识别问题，利用深度学习的方法帮助机器人完成回环检测。比如：决策树、SVM等。

　　……

　　最后，当回环出现以后，也不要急着就让机器人停止运动，要继续保持运动，多走重合的路，在已经完成闭合的路径上，进一步扫图完善细节。

　　继续走重合之路，完善闭环细节