SLAM总结（五）-视觉重定位、闭环检测和全局定位

1.重定位、闭环检测和全局定位简介

重定位和全局定位一般属于前端部分。放在这里是因为它们和闭环检测有一些相似之处。
重定位指在跟踪丢失后，累积漂移较小。通过重定位获得当前帧在世界坐标系的位姿，当前帧的位姿一般在丢失前的最新帧附近，随着丢失时间的增长，离丢失位姿会越来越远。
闭环检测时跟踪未丢失，但由于时间和空间距离较长，累积漂移较大。漂移越大，当前帧位姿越不准确。闭环检测通过找到历史时间的闭环帧(漂移相对当前帧小)减少漂移。从滤波的角度可以直观的理解，开环情况下不确定性会不断增大，发现闭环后当前帧可以建立和较久以前闭环帧的联系，当前帧不确定性减小，不确定性又从闭环帧开始增加。卡尔曼滤波的马尔可夫性，当前状态只与前一状态相关，非线性优化中当前状态和之前相关联的所有状态相关。
全局定位一般指在开机后在建好的地图上定位或在机器人绑架后进行定位，此时当前帧的位姿可能出现在已有地图中的任意位姿，甚至已有地图之外。有时也可以人为给定大致的初始位姿或者最后开机和关机的位置是确定的。为了更加通用，以下考虑初始位姿完全未知的情况。
一般地图中保留了关键帧的信息，其在世界坐标系的位姿是已知的。重定位、闭环检测和全局定位找到地图中与当前帧匹配的关键帧，这一步称为场景识别；找到匹配帧后还需要通过时间序列匹配等方法去除误匹配；最后可以对帧与帧进行姿态解算。若与关键帧的匹配点有3D位置则可使用3D-2D方法求得当前帧在世界坐标系的位姿。若与关键帧的匹配点无3D位置则可使用2D-2D方法求得当前帧在世界坐标系的相似位姿（尺度信息未知）。视觉SLAM中，可以使用ORB的BoW方法实现快速的重定位、闭环检测和全局定位，具体方法在ORB-SLAM2系列中再讲解。

2.场景识别

场景识别的目的是从已知地图中找出与当前帧匹配的关键帧，关键帧的位姿是已知的。评价场景识别的性能指标是准确率和召回率，两个指标一般此消彼长，相互制约，主要有以下几种方法：
1.穷举法：将当前帧与场景中的所有帧一一匹配。这种方法简单粗暴但计算复杂度随着关键帧增多而增大。因为时间相近的几帧一般比较相似，可以间隔几帧匹配一次，可以适当减少复杂度。这种方法还是不太可取，我们可以利用几何关系缩小查找范围，也可以利用外观缩小查找范围和加速匹配过程。
2、基于几何关系方法：重定位和闭环检测中当前帧的位姿是已知但不太准确的。可以使用这一位姿缩小关键帧搜索范围。搜索范围需要略大于位姿的误差。
3、基于外观方法：不考虑几何关系，通过图片外观特征确定相似度，是视觉SLAM的主流做法，视觉特征丰富，有利于匹配。计算图片间相似度的方法如下。
1）基于光度误差：直接将两图像做减法，这种方法有点费力不讨好，受光照、尺度和视线变化影响大，不太可取。
2）基于深度学习：这种方法更近似于人的判断。神经网络的输入可以是两幅图像，输出可以是相似度。
3）基于特征的方法：最典型的是利用点特征的描述子进行识别。为了加速匹配需要使用字典(dictionary)和词袋模型(BoW，Bag of Words)。字典一般由较大的数据集中各帧的特征描述子生成，由很多单词(word)组成。一个单词对应相似一类特征描述子，单词一般具有权重，格式与特征描述子一致，可用于区分不同特征。BoW用于描述一帧图像，一般由在图像中出现字典中的单词组成，是单词的向量。DBoW是一种典型的方法，在激光SLAM中的GFLIP也是相同的思路。只是替换了词(特征描述子)和BoW向量。可见该方法中重要的是设计合理的特征描述子、BoW向量和字典结构。好的字典结构可以加速查找速度。基于特征的简单的思路是：首先需要计算当前帧的BoW和根据当前帧的特征通过字典得到可能与当前帧相似的地图中关键帧的子集(远小于整个关键帧)，然后使用当前帧BoW和关键帧BoW计算相似度找出相似的关键帧，最后还需进行时间和空间一致性检验。下面详细介绍一下字典和BoW，在ORB-SLAM2会详细介绍其如何使用DBoW进行重定位的。
a）字典：字典一般由大的数据集得到的特征描述子通过离线训练得到。如果训练使用的数据集和使用环境是一样的表现会更好，若训练使用的数据集和使用环境不一样这就得看特征描述子和BoW的特性了，如视觉的DBoW两者不同的情况下表现也不错，但GFLIP两者不同的情况下表现不佳。表现不佳时最好利用使用环境的数据集生成新的字典。两者不同造成的区别主要是单词不一样(DBoW表现好可能是因为ORB描述子是256位二进制描述子且距离计算方法是汉明距离，若单词足够多，单词间差异大，即使不同环境也可能有很多相同或距离相近的单词，可打印单词出来验证一下，可否尝试自动生成二进制)、单词的权重不同(不同环境情况使用时不用权重表现是否会更好)。字典主要需要解决如下几个问题：
(1)单词的格式：单词是一类特征描述子的集合，它的格式和特征描述子一样，一般将一类特征描述子的中心作为一个单词。那么描述子必须有计算距离的公式，如ORB描述子之间的距离是汉明距离，即二进制中数值不同的位的位数。ORB词典的单词并不一定和某个出现过的特征相同，但距离是最近的。ORB单词的一个好处是：若两帧中的特征对应实际中的相同点对应一个实际点，因为光照、尺度和视角等原因两帧中特征的描述子不完全一样，有细微差别，但它们一般会属于同一个单词。
(2)字典的生成和结构：首先从数据集中可以提取出大量的特征，设其为m，然后我们可以设定需要的单词数为n，也就是类别数，一般m>>n。那么马上想到使用聚类方法，常用方法有K-means、层次距离和K-means++等。如果从当前帧得到一个特征时使用暴力方法逐一计算它与所有单词的距离，那么查找的时间复杂度是O(n),为了减小时间复杂度，可以使用树结构等结构来组织单词，树结构查找时可以获得对数复杂度，如kd树、Fabmap和Chou-Liu tree等，常用的是kd树，k是每层类别数，d是层数。kd树的最大单词容量是k^d，一般令n=k^d,查找复杂度时d，即log_k(n)。
(3)字典的作用：

• 获得单词的权重：对于训练环境和使用环境相同情况，单词权重比较有意义。计算单词权重的目的是使不同帧之间有更好的区分度。常用的计算权重的方法使TF-IDF(Term Frequency-Inverse Document Frequency)。TF直观的感觉是某个单词在某帧图像中的重要性，单词出现次数越多说明这个单词对该帧图像来说比较具有代表性，也就越重要；IDF直观感觉是某个单词在所有单词中的重要性，单词出现越多说明在各帧中都比较常见，也就更不好用来区分各帧，也就越不重要。某个单词两个值的计算公式如下：那么是否每帧图像相同的单词会不同？权重是不是固定的？在何时计算？
  对某帧图像而言，设出现过的单词类别为m，出现某个单词的次数为m_i，则：TF_i=m/m_i
  设整个数据集特征数量为m，其中属于某个单词类的特征数量为m_i，则：
  IDF_i=log(m/m_i）
• 加速单词查找和匹配速度，加速方法如下:
  1)树结构加速从特征查找对应单词的速度，由O(n)到O(log(n)
  2)单词到对应帧索引表：根据当前帧的特征，找到对应单词，然后由该索引表可以找出所有出现过该单词的帧，若两帧相似，那么它们相同的单词就会比较多，从而可以通过这个索引表缩小可能相似帧的查找范围，然后再利用BoW计算相似度。
  3)kd树中某层中节点到关键帧的索引表：在计算两帧相似度时可以加速找出出现过的相同的单词。只需比较该层中节点下在两帧出现过的单词
• 1
  b)BoW：它是用于描述一幅图像，可以用有没有出现某个单词或物体表示，或者出现某个单词或物体的次数，不在乎单词或物体出现在哪里。可以用向量表示。获得向量后的便是考虑如何计算相似度，计算相似度的公式也有很大，如向量相减、只统计两帧都出现的单词，共同出现单词越多越相似，这里一般会乘上单词的权重。BoW计算相似度的速度一般远远快于特征匹配的方法，从而也可以加速查找相似帧的速度。
  c)时间和空间一致性检验：时间一致性即当前帧和闭环帧附近的帧应该也是相似的；空间一致性在乎单词或物体在哪里出现，判断依据是是否可以解算出当前帧和闭环帧相对位姿且相对位姿不是很大，两帧已知的在世界坐标系的位姿不应该差太多，全局定位时当前帧在世界坐标系中的位姿一开始是未知的，故无法比较第二项。

3.闭环优化

检测到闭环后，需要通过优化来消除闭环内关键帧的位姿和路标点的漂移，然后可通过全局优化进一步减少或消除闭环意外帧的漂移。具体优化方法在ORB-SLAM2系列介绍。

4.闭环检测、全局优化、局部优化和位姿跟踪的协调

闭环检测和全局优化，局部优化都是为了减小漂移，保持全局一致，而跟踪只跟踪前一帧或几帧，只负责当前帧与之前帧的漂移，即局部一致
将后端优化结果传到前端的方法，前端相当于可以一直存在漂移，但相对位姿是准确的，后端结果不传不影响前端的工作，但后端拿到有漂移的结果需要先校正再用，否则优化也就没意义了