RTAB-Map 闭环检测


[TOC]

1. 概述

主要特点:

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

代码主要过程:

  • RTABMap(闭环检测)主入口函数 Rtabmap::process
  • 输入图像image及其idheader.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




^