针对常用的八叉树地图构建框架Rtab-Map进行解析。
RTAB-Map简述
RTAB-Map的目的在于提供一个与时间和尺度无关的基于外观的定位与构图解决方案该方案针对解决大型环境中的在线闭环检测问题。方案的思想在于，为了满足实时性的一些限制，闭环检测时仅仅利用有限数量的一些定位点，同时在需要的时候又能够访问到整个地图的定位点。当地图中定位点的数目使得找到定位匹配的时间超过某个阙值时，RTAB-Map就将WM中不太可能形成闭环的定位点转移到LTM 中，这样这些被转移的位置点就不参与下一次闭环检测的运算。当一个闭环被检测到时，其邻接定位点又能够重新的从LTM中取回放入 WM中，用于将来的闭环检测。
由于 LTM 中的定位点并不参与闭环检测, 因此选择 WM 中的哪些定位点转移到 LTM 中 是非常重要的。一种朴素的方法是用队列（FIFO）, 修剪掉地图中存在时间最长的那些定位 点。然而这样在机器人探索一个环境时, 就只能保存固定数量的一些定位点, 因此当闭环被 检测到之前的处理时间超出某个阈值时, 直接修剪掉地图中存在时间最长的那些定位点, 可 能会使得在将来的闭环检测中找不到定位点匹配（而本来是存在的）一种备选方案是, 随机的选择要被修剪的定位点, 但更好的方式是将易于被重新访问的那些定位点存储到 WM 中。RTAB-Map 的思想就是, 假设更频繁的被访问的定位点比其它的定位点更易于形成闭环。 这样,一个定位点被连续访问的次数就可以用来衡量其易于形成闭环的权重。当需要从 WM 转移定位点到 LTM 中时, 优先选择具有最低权重的定位点。如果具有最低权重的定位点又 有多个时, 优先选择被存储时间最长的那一个。
STM 用于观察连续图像在时间上的相似度, 并依此更新定位点的权重。WM 则是用于检 测定位点在空间上的闭环假设。RTAB-Map 闭环检测时并没有使用 STM 中的定位点, 因为多 数情况下, 最后获取的定位点大多与其最近的那些定位点相似。STM 的存储量大小 $T_{STM}$ 的 设定取决于机器人的速度和定位点获取的频率。当定位点数量达到 $T_{STM}$ 时, 在 STM 中存储 时间最长的定位点就被移动到 WM 中。RTAB-Map 用离散贝叶斯过滤器来估计形成闭环的概 率, 将新的定位点与存储在 WM 中的定位点进行比较。当发现新旧定位点之间有高概率形 成闭环时, 一个闭环就被检测到了, 新旧定位点也就被链接在一起。为了保证易于形成闭环 的定位点能被存储在 WM 中, 同时保证 WM 中存储的定位点数量使得贝叶斯过滤器能够实 时处理, RTAB-Map 设置了两个关键步骤。第一个步骤叫做 “取回”: 对于具有形成闭环概率 最高的那个定位点, 将它的那些没有在 WM 中的邻接定位点, 从新从 LTM 中取出放回到 WM 中。第二个步骤叫做 “转移”: 当闭环检测的处理时间超过了阈值 $T_{time}$, 具有最低权重 的定位点中, 存储时间最长的将被转移到 LTM 中, 被转移的定位点的数量取决于当前一轮 循环中 WM 存储的定位点的数量。
A.定位点创建
RTAB-Map 使用词袋方法来创建图像的签名， $z_{t}$ 表示在 $t$ 时刻获取的图像的签名。一副图 像的签名由视觉词典中的词的一个集合来表示的, 这个视觉词典是在线增量式的创建的。选择增量式的创建方法, 而不是采用预先训练好的词典, 好处在于针对一个特定的环境不需要 预训练过程。
RTAB-Map 基于 OpenCV 从图像中提取 SURF 特征来得到视觉单词。这样, 视觉词典的每一个单词就指的是一个 SURF 特征描述（一个 64 维的向量)。每一个 SURF 特征都有一个作为特征的响应力度。特征响应可以用来选择图像中最突出的那些特征。为了避免选择到一些
数目不超过 $T_{maxFeatures}$ 的 SURF 特征被保留下来, 这样每帧图像差不多都能得到同样数目的 单词。当提取到的特征的数目很少时 (相比于每帧图像的平均特征数目的比例小于阈值 $T_{bad}$ ), 那么这样的签名就被认为是一个很差的签名, 将不会被用于闭环检测。当获取的图像中不包 含具有判别能力的特征时这种情况就会发生, 比如室内一堵白墙的图像。
为了构建一个好的签名, 找到与词典中已有单词之间的匹配 (这个步骤也叫做量化处理), RTAB-Map 在 SURF 特征间的比较采用最近邻和次近邻的比率 ( nearest neighbor distance ratio, NNDR）。如果一个特征到其最近邻的距离比到其次近邻的距离的 $T_{NNDR}$ 倍要小, 那么这个特 征就能够被其最近邻的特征单词所表示。由于 SURF 特征描述具有较高维度, 一个由四个随机化的 kd-tree（FLANN）构成的分类森林被用于提高最近邻匹配的速率。kd-trees 的每一个叶子节点就代表了词典中的一个单词。随机化的 kd-tree 的构建是基于分层 k-means 聚类的, 这样创建树的时间将被大大减低。由于 FLANN 并不提供对其索引进行增量式的改变的接口 (比如随机化的 kd-trees 或分层 k-means tree), 因此每当词典被修改时, 分类森林也要重新 创建。kd-trees 是基于词典中所有单词的 SURF 特征描述来创建的。而后,一帧图像中的每 一个特征描述是通过找到 kd-trees 中最近的两个叶子节点来进行量化的。对于每个提取的特 征, 如果其不能满足 $T_{NNDR}$ 标准, 那么该特征的描述就作为一个新的单词加入到词典和 $z_t$ 中。 如果最近邻匹配满足 $T_{NNDR}$ 标准, 对应词典中的单词就被加入到 $z_t$ 中。
这样一个定位点 $L_t$ 就可以用签名 $z_t$ 和时间索引 $t$ 来创建, 其初始化的权重为 0 , 并与 $L_{t-1}$ 在 图中建立一个时间上的双向链接。
B.权重更新
为了更新获取到的定位点的权重, 将 $L_{t}$ 与 STM 中的最后一个定位点进行比较, 相似度 $s$ 通 过式 (1) 来衡量: \(s\left(z_t, z_c\right)= \begin{cases}\frac{N_{pair}}{N_{zt}}, & \text {if}\left(N_{zt} \geq N_{zc}\right) \\ \frac{N_{pair}}{N_{zc}}, & \text { if }\left(N_{zt}<N_{zc}\right)\end{cases}\) 其中 $N_{pair}$ 表示定位点签名间匹配上的单词对的数量, $N_{zt}$ 与 $N_{zc}$ 分别对应签名 $z_t$ 与 $z_{cd}$ 的总单词数目。如果 $s\left(z_t, z_c\right)$ 超过了一个固定的相似度阈值 $T_{similarity}$, 那么被比较的定位点 $L_c$ 就被融 合到 $L_t$ 中。融合后的签名中只保存了来自 $z_c$ 的单词, 而词典中新增的来自 $z_t$ 的单词则被删除 了: $z_t$ 被清空, 将 $z_c$ 复制到 $z_t$ 中。清空 $z_t$ 的原因在于, 删除词典中新增的来自 $z_t$ 的单词会更容 易, 因为在 kd-trees 中还没有为它们建立索引。通过实验观察发现, 仅保留 $z_t$ 中的单词和保 留两者融合后的单词, 在分别取不同阈值的情况下具有相似的效果。就所有的情况而言, RTAB-Map 在融合后的签名中保留了在 $z_c$ 和 $z_t$ 中均存在的单词, 因为这种情况具有较强的判 别能力, 而其它的情况通常判别能力比较弱。最后 $L_t$ 的权重设置为 $L_c$ 的权重再加一, 而且 $L_c$ 的 邻接和闭环链接重新链接到 $L_t, L_c$ 则从 $STM$ 中删除。
C.贝叶斯过滤器更新
贝叶斯过滤器的作用是通过估计当前定位点 $L_t$ 和存储在 $WM$ 中的定位点形成闭环的概 率来记录闭环假设。设 $S_t$ 为在 $t$ 时刻所有闭环假设的随机变量。 $S_t={i}$ 表示 $L_t$ 与一个已经被访 问过的定位点 $L_i$ 形成闭环, 也就是 $L_t$ 与 $L_i$ 是同一个定位点。 $S_t=-1$ 表示 $L_t$ 是一个新的定位点。
过滤器估计的是所有的 $i=-1, \ldots, t_n$ 的后验概率分布 $p\left(S_t \mid L^t\right)$, 其中 $t_n$ 是存储在 $WM$ 中最新的 定位点在时间上的索引, 计算方式如下:
\[p\left(S_t\mid L^t\right)=\rho p\left(~L_t \mid S_t\right) \sum_{i=-1}^{t_n} {p}\left(S_t \mid S_{t-1}=i\right) p\left(S_{t-1}={i} \mid L^{t-1}\right)\]其中 $\rho$ 是一个标准化的系数, $L^t=L_{-1}, \ldots, L_t$ 是一个定位点序列。注意定位点序列 $L^t$ 仅仅包含了来自 WM 和 STM 中的定位点。不同于经典的贝叶斯过滤器中 $L^t$ 是一个定长序列的情况, 这里的 $L^t$ 会不断的变化, 因为新的定位点会不断的被创建, 而且 WM 中的定位点也可能会被 转移到 LTM 中, 或重新从 LTM 中取回来。 $p\left(L_t \mid S_t\right)$ 叫做观察模型, 其本质是衡量定位点 $L_t$ 与 $S_t$ 定位点的相似度。为了区分与不同定位点的相似度之间的差异, 这里采用了一个似然函数 $\gamma\left(S_t \mid L_t\right)$ ：用式（1）将当前定位点 $L_t$ 与对应的可能形成闭环的定位点 $S_t=j\left(j=0, \ldots, t_n\right)$ 进行比较, 得到对应的相似度 $s_j=s\left(z_t, z_j\right)$, 然后每个 $s_j$ 与它们的标准差 $\sigma$ 之间的差异, 通过所有不为零的 $s_j$ 的均值来标准化。计算方式如下: \(p\left(L_t\mid S_t=j\right)=\gamma\left(S_t=j \mid L_t\right)=\left\{\begin{array}{cc} \frac{S_j-\sigma}{\mu}, & \text { if } s_j \geq \mu+\sigma \\ 1, & \text { otherwise } \end{array}\right.\) 对于 $L_t$ 是一个新定位点的可能性, 计算方式如下: \(p\left(L_t \mid S_t=-1\right)=\gamma\left(S_t=-1 \mid L_t\right)=\frac{\mu}{\sigma}+1\) 这个计算与相似度的均值与标准差之间的比率有关。如果 $\gamma\left(S_t=-1 \mid L_t\right)$ 比较大（也就是 $L_t$ 并 不与 WM 中某个特定的定位点相似, 因为 $\sigma<\mu)$, 那么 $L_t$ 很有可能就是一个新的定位点。 $p\left(S_t\mid S_{t-1}=i\right)$ 叫做转移模型, 是在已知一个 $S_{t-1}$ 的分布条件下用来预测 $S_t$ 的分布的, 这 与机器人在时刻 $t-1$ 到时刻 $t$ 间的移动是类似的。与 $p\left(S_{t-1}={i} \mid L^{t-1}\right)$ (也就是过滤器中递推的一部分)一起, 构成了下一轮闭环检测的置信值(这个置信值本质上计算的是一个全概率, 也就是在给定条件下产生某个结果的概率)。转移模型的计算方式如下:
1) $p\left(S_t=-1 \mid S_{t-1}=-1\right)=0.9$, 在时刻 $t-1$ 没有闭环发生的条件下, 时刻 $t$ 产生了一个新 的定位点的概率。 2) $p\left(S_t=i \mid S_{t-1}=-1\right)=\frac{0.1}{~N_{WM}}\left(i \in\left[0, t_n\right]\right)$, 在时刻 $t-1$ 没有闭环发生的条件下, 时刻 $t$ 与 $i$ 发生了一个闭环的概率。 3) $p\left(S_t=-1 \mid S_{t-1}=j\right)=0.1\left(j \in\left[0, t_n\right]\right)$, 在时刻 $t-1$ 与 $j$ 发生闭环的条件下, 时刻 $t$ 产生 了一个新的定位点的概率。 4) $p\left(S_t=i \mid S_{t-1}=j\right)\left(i, j \in\left[0, t_n\right]\right)$, 在时刻 $t-1$ 与 $j$ 发生闭环的条件下, 时刻 $t$ 与 $i$ 发生闭环 的概率。这个概率由离散化的以 $j$ 为中心的高斯曲线来定义的, 也就是中心点 16 邻域范 围内($i=j-16, \ldots, j+16)i$ 不为空时对应的值。在拓扑图中有些定位点可能会有超过两个的邻接定位点 (如果之前形成了闭环的话), 而这些邻接定位点也可能并不完全都在 WM 中(因为他们被转移到了 LTM 中 ) 。最后 ${p}\left(S_t\geq 0 \mid S_{t-1}=j\right)$ 被归一化到和为 $0.9$ 的分布值。
D.闭环假设选择
当 $p\left(S_t\mid L^t\right)$ 被计算且被归一化完成后, 如果 $p\left(S_t=-1 \mid L^t\right)$ 比设定的闭环或值 $T_{loop}$ 小, 那么 $p\left(S_t \mid L^t\right)$ 中具有概率最大值的闭环假设 $S_t=i$ 就被认为是成立的。当一个闭环假设被认为成立 时, 新的定位点 $L_t$ 就与旧的定位点 $L_i$ 之间就建立了闭环链接: $L_t$ 的权重更新为原来的权重加上 $L_i$ 的权重, $L_i$ 的权重设置为零, 添加 $L_i$ 到 $L_t$ 的闭环链接。闭环链接的作用在于, 在取回时 用于找到旧定位点的邻接节点, 以及用于贝叶斯过滤器中的转移模型的计算。值得注意的是, 的定位点也不再融合, 仅仅在它们之间添加一条链接。不对定位点进行融合带来的好处是能 够保存到同一定位点的不同的签名, 这能够带来对将来闭环假设更好的估计, 这在高度动态 变化的环境中或者当环境随时间循环变化 (比如白天到黑夜或者天气变化等) 时非常重要。
E.取回
闭环检测完成后, 具有形成闭环概率最高的定位点的那些没有在 WM 中的邻接定位点, 会重新从 LTM 取回到 WM 中。
当定位点从 LTM 中取回后, 就会用被取回的定位点对应的签名去更新词典中的单词。被 取回的签名中一些常用的词汇仍然存在于词典中, 这样对应签名与词典中相关单词已经被添 加了引用。而对于那些没有存储在词典中的词 (因为当定位点被转移到 LTM 中时, 对应签 名的词也从词典中被删除了), 它们的量化方式仍然和前面 A 中讨论的一样（但是, 要使用 kd-trees 与词典中的词进行匹配以及与词典中新增的还没有添加 kd-tree 索引的词线性的进 行匹配), 看是否与最近词典中的新词具有相同的特征描述。这一步是很重要的, 因为词典 中新增的来自签名 $z_t$ 的词, 可能与被转移的词具有相同的特征描述。对于被取回的签名中的 词, 如果有了新的匹配, 那么这个词就用新的匹配上的词替换。之所以 LTM 中签名与词典 中的词的引用没有立刻被改变, 是因为这个操作的计算量是很大的。然而当一些签名从 LTM 中被取回到 WM 中时, 对应的引用就会被改变。而且, 当整个系统被关闭的时候, LTM 中 的所有引用将会被清空。如果取回后的签名中仍然有一些词匹配不上, 那么它们将直接被加 入到词典中。
由于从数据库中加载定位点事很消耗时间的, 因此一次循环中最多取回两个定位点（在 C 中定义的邻接范围内）。当超过两个定位点可以被取回时, 在时间上邻接的定位点（闭环 假设定位点的直接邻接定位点) 优先于在空间上的邻接定位点。这中策略在机器人运动的时 候是很重要的, 因为取回在时间上的邻接定位点比在空间上邻接的定位点更合适。然而当机 器人静态等待一会后, 所有在时间上的邻接定位点将会被取回, 紧接着便是在空间上的邻接 点被取回。
F.转移
当处理一帧图像的时间超过 $T_{time}$ 的时候, 具有最低权重中被存储时间最长的定位点就会 从 WM 中被转移到 LTM 中。为了使离散贝叶斯过滤器能够合理的估计闭环假设, 具有最高 闭环假设定位点的邻接定位点是不允许被转移的。为了避免对空间上的邻接定位点不敏感, 这些定位点的数目受限于其在时间上的邻接定位点的数目（按照在 C 中定义的）。 $T_{time}$ 是通 过实验经验来设置的, 使得能够对感知到的图像进行实时在线的处理。较高值得 $T_{time}$ 意味 着更多的定位点 (直观一点就是更多的词) 会存储在 WM 中, 这样更多的闭环假设能够被 保存起来, 也就更好的代表了整个环境。因此 $T_{time}$ 的设置会参照机器人 $CPU$ 的配置, 计算 复杂度以及操作的环境。如果设置 $T_{time}$ 比获取一帧图像的时间高一点, 该算法本质上能够 百分百的使用 CPU 的条件下达到以 $T_{time}$ 为基准的帧率。因为在 RTAB-Map 中计算时间代价最高的步骤是创建最近邻索引(也就是创建 kd-trees),
因此处理获取的每一帧图像的时间可以通过改变词典的大小来限制管理, 词典的大小也间接 的影响了WM 的大小。当一个定位点的签名从 WM 中转移到 LTM 中时, 这个签名中对应词 的引用也就被清除了。如果一个词没有被 WM 中任何一个签名所引用, 那么这个词就被转 移到 LTM 中。当从词典中转移出去的词的数量比从 $L_t$ 或者被取回定位点中添加的词的数量少 时, 这就意味着更多的定位点已从 WM 中被转移到 LTM 中了。当转移处理完成时, 词典的 大小要比在词典中加入了来自 $L_t$ 或者被取回定位点中的词之前要小，因此降低了为下一帧图 像处理所需而创建 kd-trees 的时间。将被转移的定位点存储到数据库中是通过一个后台线程 异步完成的，这导致了在下一次循环前需要一定量的时间延迟。
在 WM 达到其最大存储量时, 转移定位点到 LTM 中的方式直接影响了在长时过程中的 处理, 尤其当一个区域 (一个定位点集合）比其它区域更频繁的被访问时。通常的,一个新 访问区域中至少有一个定位点会通过权重更新得到一个高权重, 进而替代在 WM 中旧的具 有高权重的定位点, 这样是为了在重新访问这个区域时能够检测到回环。然而, 如果一个新 访问的区域没有一个较高权重的定位点来代表它, 除非机器人回到一个旧区域中的一个高权 重定位点, 那么闭环是不会被检测到的, 然后再从那里开始到另外一个新的区域 (这里为什 么要强调这个, 个人认为机器人运动过程中, 明明重新访问了一个区域, 却检测不到闭环, 这是一件很危险的事情, 因为这样会导致误差累积)。为了解决这种情况, 在上一次闭环被 检测到后, 具有最高权重的定位点的一个子集 (由 $T_{recent} \times N_{WM}$ 来定义) 是不允许从 WM 转移到 LTM 中的。这样, 在机器人探索一个新区域时, 总会有一些具有高权重的定位点存 储在 WM 中, 直到一个闭环被检测到。如果在上一次㐽环被检测到后, 在 WM 中定位点的 WM 中, 这也意味着更多的旧的定位点会被转移到 LTM 中。