RTAB-Map 闭环检测

本文主要是介绍RTAB-Map 闭环检测，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

欢迎访问 https://cgabc.xyz/posts/999fccc5/，持续更新

1. 概述

主要特点：

基于外观（Appearance-Based），通过图像相似度查找回环
贝叶斯滤波算法，估计回环的概率
增量式在线构建视觉词典或词袋，针对一个特定环境不需要预训练过程
内存管理模型，保证实时在线运行

代码主要过程：

RTABMap（闭环检测）主入口函数 Rtabmap::process
输入图像image及其id（header.seq）被封装到SensorData类
内存更新(Memory::update)
- 创建签名(Memory::createSignature)
- Add Signature To STM(Memory::addSignatureToStm)
- Weight Update, Rehearsal(Memory::rehearsal)
- Transfer the oldest signature from STM to WM(Memory::moveSignatureToWMFromSTM)
贝叶斯滤波器更新
- 计算似然(Memory::computeLikelihood)
- 调整似然(Rtabmap::adjustLikelihood)
- 计算后验(BayesFilter::computePosterior)
- 选择最高的闭环假设
RETRIEVAL
- Loop closure neighbors reactivation
- Load signatures from the database, from LTM to WM(Memory::reactivateSignatures)
Update loop closure links: make neighbors of the loop closure in RAM
TRANSFER: move the oldest signature from STM to LTM

算法主要流程：

内存管理模型：

回环检测（若不考虑内存管理）过程：

2. 内存更新

2.1 创建签名

代码在 Memory::createSignature 中，其主要过程为

词典更新(VWDictionary::update) 线程
- 构建FLANN索引
  - 根据描述子构建KDTree索引词典 (_flannIndex->buildKDTreeIndex, kNNFlannKdTree)
  - KDTree的创建基于分层k-mean聚类
- 更新_dataTree
角点(GFTT)检测(Feature2D::generateKeypoints)
- 均匀分布(gridRows_, gridCols_)
- 限制点数(maxFeatures_)
- 亚像素提取(cv::cornerSubPix)
描述子(BRIEF)计算(Feature2D::generateDescriptors)
quantize descriptors to vocabulary(VWDictionary::addNewWords)
- 描述子匹配(descriptors – dataTree)，并计算距离
- 添加单词或参考+1
  - badDist=true（匹配数量特别少，或 NNDR(neareast neighbor distance ratio) 大于 $T_{\text{NNDR}}$ 阈值_nndrRatio），然后创建VisualWord并添加到_visualWords
  $\text{NNDR} = \frac{\text{dist of the neareast neighbor}}{\text{dist of the second-neareast neighbor}}$
  - badDist=false，VWDictionary::addWordRef，_references+1
创建签名(new Signature)

2.2 添加签名到STM

代码主要在 Memory::addSignatureToStm 中。

更新neighbors，添加链接(signature->addLink)
添加签名ID到_stMem

2.3 更新权重

代码主要在 Memory::rehearsal 中。

signature与STM中最新的签名比较，计算相似度RehearsalSimilarity(float sim = signature->compareTo(*sB))

$\text{similarity} = \frac{\text{pairs}}{\text{totalWords}}$

相似度 > 阈值_similarityThreshold，假设合并(Memory::rehearsalMerge)，更新权重
- 更新权重(signature->setWeight)
$w_A = w_A + w_B + 1$
- 新旧签名合并(Memory::rehearsalMerge)
  - 新签名添加Link
  - 删除旧签名(moveToTrash)

2.4 签名转移 (STM->WM)

Transfer the oldest signature of the short-term memory to the working memory

_maxStMemSize = 10
Memory::moveSignatureToWMFromSTM:_workingMem.insert 和 _stMem.erase

3. 贝叶斯滤波器更新

计算似然(Memory::computeLikelihood)，得到 rawLikelihood
- 算法一：tf-idf (term frequency–inverse document frequency)
$\text{tf-idf} = \frac{n_{wi} + \log \frac{N}{n_w}}{n_i}$
- 算法二：相似度 RehearsalSimilarity
调整似然(Rtabmap::adjustLikelihood) $s_j$ ，得到 likelihood
- 依据似然均值 $\mu$ 和似然标准差 $\sigma$
$\text{likelihood} = \begin{cases} \frac{s_j - \sigma}{\mu}, \quad \text{if} \ s_j \geq \mu + \sigma \\ 1, \quad\quad\quad \text{otherwise}. \end{cases}$
计算后验(BayesFilter::computePosterior)，得到 posterior
- 预测(Prediction : Prior*lastPosterior)得到_prediction
- 更新后验(BayesFilter::updatePosterior)
- 计算先验(prior = _prediction * posterior)
- 计算后验
- 后验归一化

$\text{posterior} = \text{likelihood} \times \text{prior}$

4. 回环假设选择

Select the highest hypothesis

根据后验posterior选择最高的假设_highestHypothesis
接受回环，条件如下：
- _highestHypothesis > _loopThr
- 对极几何 检查(_epipolarGeometry->check)
  - 单词匹配对数量(EpipolarGeometry::findPairsUnique) > _matchCountMinAccepted
  - 对极约束内点数(EpipolarGeometry::findFFromWords) > _matchCountMinAccepted（RANSAC方法计算基础矩阵）

5. 取回 Retrieval (LTM->WM)

对于形成回环概率最高的定位点，将他那些没有在WM中的邻接定位点，从LTM中取回放入到WM中。

Loop closure neighbors reactivation
- time
- space
Update planned path and get next nodes to retrieve
Load signatures from the database (LTM->WM)

6. 转移 Transfer (STM->LTM)

If time allowed for the detection exceeds the limit of real-time, move the oldest signature with less frequency entry (from X oldest) from the short term memory to the long term memory.

具有最低权重的定位点中，存储时间最长的将被转移到LTM（数据库SQLite）中。

参考文献

[1] Appearance-Based Loop Closure Detection for Online Large-Scale and Long-Term Operation
[2] Fast and Incremental Method for Loop-Closure Detection Using Bags of Visual Words

这篇关于RTAB-Map 闭环检测的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！