RTAB-MAP原理详解
背景知识RGBD-SLAM基本流程
目的
提供一个与时间和尺度无关的基于外观的定位与构图解决方案。
针对解决大型环境中的在线闭环检测问题。
思想
为满足实时性的一些限制，闭环检测是仅仅利用有限数量的一些定位点，同时在需要的时候又能够访问到整个地图的定位点。当地图中定位点的数目使得找到定位匹配的时间超过某个阀值时，RTAB-MAP就将WM(Working Memory)中不太可能形成闭环的定位点转移到LTM（Long-Term Memory）中，这样这些被转移的位置点就不参与下次闭环检测的运算。当一个闭环被检测到时，其领接定位点又能够重新的从LTM中取回放入WM中，用于将来的闭环检测。
由于LTM中的定位点并不参与闭环检测，因此选择WM中的哪些定点转移到LTM中非常重要。
rtab-map的思想是：假设更频繁的被访问的定位点比其他的定位点更易于形成闭环。这样一个定位点被连续访问的次数就可以用来衡量其易于形成闭环的权重。当需要从WM转移定位点到LTM中时，优先选择具有最低权重的定位点。如果具有最低权重的定位点又有多个时，优先选择被存储时间最长的那一个。
STM(Short-Term Memory)用于观察连续图像在时间上的相似度，并依此更新定位点的权重。
WM用于检测定位点在空间上的闭环假设。
RTAB-MAP闭环检测时并没有使用STM中的定位点，因为多数情况下，最后获取的定位点大多与其最近的定位点相似。
STM的存储量大小T取决于机器人的速度和定位点获取的频率，定位点数量达到t时，在STM中存储时间最长的定位点就被移动到WM中。
RTAB-Map用离散贝叶斯过滤器来估计形成闭环的概率，将新的定位点与存储在WM中的定位点进行比较。当发现新旧定位点之间有高概率形成闭环时，一个闭环就被检测到了，新旧定位点也就被链接在一起。有两个关键个步骤，一个是“取回”:对于具有形成闭环概率最高的那个定位点，将它的那些没有在WM中的领接定位点，重新从LTM中取出放回到WM中。第二个步骤叫做“转移”:当闭环检测的处理时间超过阀值,具有最低权重的定位点中，存储时间最长的将被转移到LTM中，被转移的定位点的数量取决于当前依赖循环中的WM存储的定位点的数量。
A. 定位点创建 Location Creation
使用词袋法创建图像的签名，一幅图像的签名由视觉词典中的词的集合表示，是在线增量式创建的。
优点：选择增量式的创建方法，而不是采用预先训练号的词典，好处在于针对一个特定的环境不需要与训练过程。
词袋方法：
第一步：利用SURF算法从不同类别的图像中提取视觉词汇向量，这些向量代表的是图像中局部不变的特征点；
第二步：将所有特征点向量集合到一块，利用K-Means算法合并词义相近的视觉词汇，构造一个包含K个词汇的单词表；
第三步：统计单词表中每个单词在图像中出现的次数，从而将图像表示成为一个K维数值向量
基于opencv从图像中提取SURF特征(阀值超过Tresponse)来得到视觉单词。
注意:当提取到的特征的数目很少时（小于阀值Tbad）被认为是一个很差的签名，将不会被用于闭环检测。比如室内一堵白墙的图像。
量化处理（找到词典中已有单词之间的匹配），rtab-map中采用最近邻和次近邻的比率NNDR,如果一个特征到最近邻的距离比到次近邻的距离的Tnndr倍药效，那么这个特征就能够被其最近邻的特征单词所表示。由四个随机化的kd-tree(FLANN)构成的分类森林来提高最近邻匹配的速率。kd-tree的构建是基于分层k-means聚类的，这样可以减低创建树的时间。
这样一个定位点Lt就可以用签名Zt和时间索引t来创建，初始权重0，并与Lt-1在图中创建一个时间上的双向链接。
B. 权值更新 Weight Update
为了更新获取到的定位点的权重，将Lt和STM中的最后一个定位点进行比较，相似度s通过（1）式来衡量
Npair表示定位点签名间匹配上的单词对的数量，Nzt与Nzc分别对应签名Zt与Zc的总单词数目。如果s超过固定的相似度阀值Tsimilarity,则将被比较的定位点Lc被融合到Lt中。融合后的签名中只保存来自Zc的单词，二词典中新增的来自Zt的单词会被删除：Zt被清空，将Zc复制到Zt中。
最后Lt的权重设置为Lc的权重再加1，而且Lc的邻接和闭环链接重新链接到Lt,Lc则从STM中删除。
C. 贝叶斯过滤器更新 Bayesian Filter Update
作用：估计当前定位点Lt和存储在WM中的定位点形成闭环的概率来记录闭环假设。
D. 闭环假设选择 Loop Closure Hypothesis Selection
归一化后如果p比设定的闭环阀值Ttop小，那么p中具有概率最大值的闭环假设St=i就被认为是成立的。当一个闭环假设成立时，新的定位点Lt就与旧的定位点Li之间就建立了闭环链接：Lt的权重更新为原来的权重加上Li的权重，Li的权重设置为0，添加到Li到Lt的闭环链接。
E.取回 Retrieval
闭环检测完成后，具有形成闭环概率最高的定位点的那些没在WM中的邻接定位点，会重新从LTM取回到WM中。
注意：由于数据库中加载定位点是很消耗时间的，因此一次循环最多取回两个定位点（在C中定义的邻接范围内）。当超过两个定位点可以被取回时，时间上邻接的定位给点优先于在空间上的邻接定位点。
F. 转移Transfer
当处理一帧图像的时间超过Ttime的时候,具有最低权重的定位点中，存储时间最长的将被转移到LTM中。
注意为了使离散贝叶斯过滤器能够合理的估计闭环假设，具有最高闭环假设定位点的邻接定位给点是不允许被转移的。
Ttime是通过实验经验来设置的，较高值的Ttime意味着更多的定位点会存储在WM中，这样更多的闭环假设能够被保存起来，也就更好的代表了整个环境。
因为rtab-map中计算时间代价最好的步骤是创建kd-tree,因此处理获取的每一帧图像的时间可以通过改变词典的大小来限制管理，词典的大小也间接影响了WM的大小。