CL关于ORB_SLAM的那些事（ORBmatcher）

ORBmatcher部分的学习

昨天读了关键点提取的每一行代码，今天当然是读匹配方面的代码了，开始今天的学习，现在是北京时间2019年08月09日上午7点49分。

按照规矩，先来看一下，该类中维护的变量和方法。
这里面就只有一个ORBmatcher的类，今天注意了一下该头文件内所包含的其他头文件，有MapPoint、KeyFrame和Frame，哪里会用到呢？接着往下看…

1. 自己提供的匹配构造函数，参数列表是默认参数，0.6和true
2. 计算描述子之间的汉明距离
3. 寻找MapPoint和当前帧内关键点的匹配关系，在track中会用到
4. 与3相同的函数名，但是参数列表不一样（函数重载），用于重定位，当传感器丢了之后，会将关键帧观测到的MapPoint投影到当前帧中，寻找匹配关系
5. 通过词袋寻找当前帧与关键帧之间的匹配关系，这个函数用于重定位，用的类似暴力匹配
6. 通过词袋寻找关键帧之间的匹配关系，主要是用在回环检测方面
7. 单目初始化时关键点匹配的函数
8. 匹配用于三角化MapPoint，这边主要是用到了极线约束
9. 寻找匹配关系，用于计算sim3，MapPoint必须是在两帧关键帧中都能被观测到
10. 将MapPoint投影到关键帧中，寻找重复出现的MapPoint
11. 对10进行了重载，用计算得到的sim3将MapPoint投影到关键帧中，寻找重复出现的MapPoint
    ###########我是一条分割线，分割线以下的是protected的函数方法############
12. check点到极线距离，参数列表：两个kp，基础矩阵和一帧关键帧
13. 观测视角余弦值半径
14. 计算计算三个最大值？？？？这是啥？？？

维护的变量有

1. low阈值
2. high阈值
3. histo的长度
   ###########我是一条分割线，分割线以下的是protected的变量############
4. float mfNNratio 暂时不知是啥含义
5. bool型的check角度

开始今天的重点了，function by function
首先是给三个静态变量赋值，high阈值是100，low阈值是50，histo长度为30。

ORBmatcher()
构造函数，参数列表默认参数，并对protected的两个变量进行赋值，没有具体的实现函数体。

SearchByProjection()
参数列表：当前图象帧、MapPoint指针类型的vector、常float阈值
匹配数目初始化为0
循环遍历MapPoint，一方面，判断该MapPoint是否在视野中用于跟踪；另一方面，判断这个MapPoint是否是坏点（这边对于坏点的定义等看到MapPoint之后再解释，这边先知道有这么个判断依据）。只有排除这两种情况，该MapPoint才有资格被考核。函数往下执行就是获取该MapPoint跟踪的尺度层，调用RadiusByViewingCos()计算观测窗口的半径。最后得到的观测窗口的半径需要根据th与1的关系，来决定是否要与th相乘，得到最终的半径。
调用Frame中的GetFeaturesInArea()函数，获取以该MapPoint投影位置为中心，r范围内的特征点，其实这些点都是与该点匹配的候选点。
获取该MapPoint的描述子，对候选点进行遍历，找出最匹配的第二匹配的点。具体的算法是：

1. 记录该关键点的index
2. 判断这个关键点是否被三角化成MapPoint，如果是被三角化之后，继续判断其被观测过几次。如果被多次观测到，就说明这个关键点与某一个关键点已经是较好的匹配了，那么这个点不可能与现在这个MapPoint有匹配关系，直接continue。
3. 判断该点在右图中的值，如果在右图中的值大于0，继续判断该点在右图中的投影位置，计算投影位置x方向与右图上本身的值的差值，将这个值与观察窗口尺寸r作比较，如果大于的话，就说明在该MapPoint的视角中就不会观测到这个关键点，直接continue
4. 通过重重考核，现在获取该关键点的描述子，然后调用DescriptorDistance()函数，计算这个描述子之间的汉明距离，距离越小，就表明这两个点越相似。
5. 比较获得最匹配和次匹配，最匹配的会记录下它的index
6. 如果最匹配的距离小于等于high阈值，就会判断最匹配和次匹配的点所在的金字塔层数是否相同以及最匹配的距离是否大于mfNNratio因子下的次匹配距离，这个MapPoint没找到匹配，继续循环下一个MapPoint寻找匹配
7. 通过所有考核的MapPoint将被放入该关键点索引index处的mvpMapPoints内，然后匹配点数量自增1。
8. 最后返回匹配上的关键点数量

RadiusByViewingCos()
根据观测的角度的余弦值来给定r的大小
如果接近零度时，r为2.5
不然，r为4.0

CheckDistEpipolarLine()
check极线距离，参数列表为两个关键点，基础矩阵和关键帧
将kp1通过基础矩阵映射到kp2图像中的极限方程
利用点到直线的距离公式，就按kp2到kp1极线的距离，程序里是计算了距离的平方。如果距离小于一定的阈值，就返回true，不然就是不符合极线约束，返回false。

SearchByBoW()
参数列表：关键帧、当前帧和MapPoint类指针的容器

1. 从关键帧中获取其包含的MaPoint的匹配关系
2. 定义了MapPoint匹配的容器，初始化为NULL
3. 获取该关键帧的词汇特征
4. 定义了matches匹配点对数
5. 定义了rotHist的数组容器，看这个变量的命名，应该是用于生成直方图的
6. 定义了因子，HISTO的长度的倒数
7. 定义关键帧和当前帧词袋方面的迭代器，从begin到end
8. 循环遍历关键帧和当前帧中各个词袋信息。

这里面暂时还没了解到词袋方面的知识，所以这边暂时是从代码的角度进行理解，带有一种猜的感觉。所以…
在循环中，它先判断词袋里的first的状态，分了三种情况，两者相等、关键帧的比当前帧小、其他
在两者相等的情况下，获取second的内容，通过second来找到关键帧中的MapPoint的index，这对后面描述子的获取很重要。对MapPoint的判断和上文的寻找匹配类似，然后循环计算描述子之间的距离，下面的过程与上文类似，不同的是这边还要进行角度的核对，计算匹配上的点角度之差。根据角度差生成一张直方图，bin从0到30，nmatches++
在关键帧的first小于当前帧的first时，将KFit = vFeatVecKF.lower_bound(Fit->first);
其他情况就正好反过来。
最后调用ComputeThreeMaxima()计算得到三个索引值，剔除一些误匹配。
最后返回匹配上的点对数。

SearchByProjection()
虽然这个函数和上文的函数名一样，但是这边的函数参数列表为：关键帧、sim3(带尺度信息的Se3)、MaPPoint、点对的匹配关系和一个阈值，这个函数主要是用在loop closure上的。具体的算法过程：

1. 取关键帧的内参
2. 从sim3中分解出带尺度的旋转矩阵、尺度、去尺度的旋转矩阵、去尺度的平移矩阵和传感器中心的世界坐标
3. set内存放匹配上的MapPoint
4. 初始化nmatches为0
5. 取出该关键帧中已经找到的MapPoint，后面如果发现已经有了的话就不用考虑了。
6. 获取该MapPoint的世界坐标并将其转换到传感器坐标系中
7. 判断该点的深度值是否为负
8. 将该点进行投影获取它的图像坐标
9. 判断该点是否还在图像内
10. 计算该点的距离尺度不变区域
11. 判断这个点是否在该区域内
12. 观测角度也必须小于60度
13. 根据距离，计算该点在该关键帧下的层数
14. 根据层数获取其尺度因子，计算其有可能的匹配区域
15. 获取该区域内的特征点，判断该点是否已经有匹配关系，判断该点所在的金字塔层数，计算描述子之间的距离，找出组匹配的index
16. 最后确定是否能匹配的上，还需要最匹配的距离小于规定的阈值
17. 最后返回匹配上的点对数量

SearchForInitialization()
这个函数是用在单目初始化的特征点匹配
参数列表：两帧Frame、之前的匹配点、1到2的匹配关系和窗口的大小
初始化nmatches为0
初始化1到2的匹配关系为-1
定义 rotHist直方图信息数组
初始化匹配距离和2到1的匹配关系
找出之前匹配的在F2中的区域，取得该区域内的所有特征点，计算两个描述子之间的距离，获得最匹配和次匹配。
两个距离阈值判断，判断2到1是否有匹配，如果没有就直接添加匹配关系；如果有，就将原来1到2的匹配剔除。反正就是挑距离最小的匹配点。
进行角度的check，这边也是调用了ComputeThreeMaxima()函数，进行了index的计算，具体计算过程等看到这个函数再了解。这边也相当于是对匹配关系进行剔除。
最后更新之前匹配的点
返回匹配上的点对数

SearchByBoW()
参数列表：两帧关键帧和1到2的匹配关系
整个函数与上文的同名函数相比，除了参数列表之外，其中的算法过程是一模一样的，所以参考上文的同名函数就可以了。

SearchForTriangulation()
该函数的参数列表：两帧关键帧、两帧之间的基础矩阵、匹配点对和只是双目的标志位。

1. 取两帧的特征向量
2. 取第一帧世界坐标系下的传感器中心坐标
3. 取第二帧传感器的旋转和平移矩阵，计算第二帧传感器的中心坐标，通过投影矩阵得到图像中心的坐标吗？？？？不了解额
4. 准备开始各种匹配的参数
5. 匹配过程和用词袋的差不多，也是分了三种情况，主要是first相同的情况，如果右图有值，就将bStereo1标记为true，如果传入的参数bOnlyStereo为true，但是bStereo1为false，那么这个MapPoint将不会被匹配
6. 取那个MapPoint对应的关键点和关键点的描述子
7. 取第二帧内的MapPoint、对应的关键点和关键点的描述子，如果该MapPoint已经有了匹配点或者是单独MapPoint，这个MapPoint将会被忽略
8. 如果该点在右图中有值，就将bStereo2标记为true，如果传入的参数bOnlyStereo为true，但是bStereo2为false，那么这个MapPoint将不会被匹配
9. 取那个MapPoint对应的关键点和关键点的描述子
10. 计算两个MapPoint之间的相似度，先剔除一些相似度很小的点
11. 当相似度满足一定的阈值要求之后，继续进行细致地考量： 这边的有一个if语句，只有当bOnlyStereo为false时，且这两帧图像在右图上都没有像素值时，才会执行。它要保证被匹配的那个关键点离图像中心的距离大于100倍的尺度因子。
12. 检查极线距离，满足要求的就记录下匹配上的关键点索引和距离值
13. 和上面的匹配函数类似，用直方图进行误匹配的剔除。
14. 最后记录匹配信息，返回匹配的点对数

Fuse()
函数的参数列表：一帧关键帧、MapPoint和常阈值

1. 获取当前关键帧的旋转矩阵和平移矩阵
2. 取其传感器参数的内参
3. 取该关键帧的传感器中心坐标（世界坐标系下）
4. 初始化nFused为0
5. 查看现有MapPoint的数量
6. 循环遍历现有的所有MapPoint，判断一下点是否在关键帧中。
7. 获取该点在世界坐标系下的坐标，计算转换后的坐标，该点的深度值必须是大于0
8. 将其投影到图像坐标系下，判断其是否在图像中，换算到右图的u坐标，获取尺度不变的距离范围。
9. 获取该点与传感器中心的矢量坐标，计算得到距离
10. 观测该点的传感器视角必须是小于60度
11. 根据距离信息和关键帧信息猜测该点所在的金字塔层数
12. 再根据层数来大概猜测观测区域的尺寸，获取该观测区域的所有的特征点
13. 获取当前匹配点的描述子，循环遍历那些待匹配的关键点，先是判断两者是否处于同一层的金字塔，再看一下该待匹配点在右图中的值是否大于等于0
14. 如果满足条件，在考虑距离时加入右图值；如果不满足条件，就直接是像素坐标的距离。阈值比较时，要考虑尺度因子，如果不满足阈值要求，那么这个点就不是匹配点。
15. 计算描述子之间的汉明距离，选出相似度最高的匹配点
16. 对最匹配的距离值与low阈值作比较，如果小于等于该阈值，那么取出这个关键点的MapPoint，判断一下这个定是否是MapPoint，如果是且这个点是好点，另外观测的次数也比它多，就用这个MapPoint来代替原先的MapPoint，否则就反过来代替。如果这个关键点对应的没有MapPoint，那么该输入MapPoint也将被该关键帧观测并在该关键帧中添加这个MapPoint。nFused++

Fuse()
该函数的参数列表：一帧关键帧、sim3矩阵、MapPoint的容器、阈值和要代替的MapPoint容器

1. 取关键帧的传感器内参
2. 分解sim3矩阵，获得旋转矩阵、尺度因子、去尺度因子的旋转矩阵和平移矩阵、最后是世界坐标系下的传感器中心坐标
3. 获取现在关键帧中包含的MapPoint
4. 初始化定义nFused为0
5. 循环遍历传入参数中的MapPoint，抛弃坏的MapPoint，如果该MapPoint已经在关键帧中了，那这个MapPoint也将会被抛弃
6. 下面的算法过程和上面的函数Fused()差不多

SearchBySim3()
该函数的参数列表：两帧关键帧、1到2的匹配MapPoint容器、1到2的尺度、1到2的旋转矩阵、1到2的平移矩阵和阈值

1. 关键帧1的传感器内参、关键帧1和2的旋转平移矩阵
2. 带尺度的1到2的旋转矩阵、带尺度的2到1的旋转矩阵和2到1的平移矩阵
3. 获取关键帧1和2的MapPoint，初始化bool类型的已匹配容器1和2(false)
4. 循环遍历传入参数中MapPoint。如果该点是MapPoint的话，这个点肯定是有匹配关系的。所以，当判断是MapPoint时，1匹配肯定是true，然后获取该点在关键帧2中的索引，判断其是否在2的MapPoint容器内，如果在，那么2匹配肯定也是true
5. 初始定义int类型的容器，用于存放匹配的索引吧
6. 寻找遍历1中还未匹配的好MapPoint，用于后续的匹配
7. 计算得到世界、传感器1和传感2坐标系下的坐标，依旧是老几样要求，深度值大于等于0，将该点投影到关键帧2中的像素平面，保证其在像素平面内。后面又是尺度不变的距离范围。(前文多次提及到尺度不变的距离范围，个人的理解是因为成像的特征是随着物距的增加而改变，在特征点提取方面也用的是图像金字塔并且，记录了每一个关键点的尺度信息，所以距离的变化会带来尺度的变化，为了保证匹配的精度，需要将尺度的信息考虑进来，所以这里提出了尺度不变的距离范围。另外，我们也可以通过这样的距离反推出大致的尺度信息，两者相互支撑，保证了匹配的精度，看完了之后还是觉得很精髓的。)
8. 再往下怎么寻找的匹配的方法与上面就很类似，就是在一定的范围寻找相似度最高的关键点，这不过这边的匹配做了两次，1到2、2到1
9. 两个各匹配了一次之后，进行一致性矫正，记录那些1到2、2到1都能匹配上的点对
10. 返回匹配上的点对数

SearchByProtection()
函数的参数列表：当前帧、前一帧、阈值和Mono模式的标志位

1. 初始定义nmatches为0
2. 定义直方图，计算各种转换之间旋转和平移矩阵
3. 向前运动还是向后运动的确定，1）双目模型 2）基线距离(可能也不是基线距离)与平移矩阵的z值的比较结果 z值肯定是大于极限距离，只不过如果是正的大于，就说明是向前，反之就是后退。
4. 循环遍历上一帧中的MapPoint，首先是MapPoint且不是外点，然后就是进行投影，是否在区域内的判断，获取该点的尺度信息，计算搜索区域。然后下边就要用到我们刚刚判断的向前运动还是向后运动了。如果是向前运动的话，寻找匹配点时的尺度范围[关键点本身的尺度，-1]；如果是向后运动，寻找匹配点时的尺度范围[0，关键点本身的尺度]；其他就是[关键点本身的尺度-1，关键点本身的尺度+1]（看到这边有一个问题，当判断是向前时，寻找的匹配尺度范围就确定不可能找到待匹配的关键点，这样做是为什么呢？？？？）
5. 获取关键点的描述子
6. 循环遍历刚刚区域内找到的关键点，判断他们是否在当前中被观测到，如果是，就说明这个点有很鲁棒的匹配关系，就不用进行这个点的匹配，另外要比较右图中的u坐标，应该是小于观测尺寸的，最后才是计算描述子的相似度
7. 调用ComputeThreeMaxima()函数来剔除误匹配
8. 返回匹配上的点对数

SearchByProtection()
函数的参数列表：当前帧、关键帧、set容器的MapPoint、阈值和ORB特征的距离

1. 初始定义nmatches为0
2. 获取当前帧的位姿信息，包括旋转、平移矩阵和传感器在世界坐标系下的坐标
3. 建立直方图容器
4. 获取关键帧中所用的MapPoint
5. 循环遍历寻找匹配关系，过程类似，只不过最后最匹配点的相似度要与ORB距离阈值进行比较，只有小于等于这个阈值的点才有可能成为那个匹配点
6. 用直方图，调用ComputeThreeMaxima()函数来剔除误匹配
7. 返回匹配上的点对数目

终于等到你，看了上面的匹配过程，每次最后都会调用此函数来进行提出误匹配，这个函数到底是何方神圣，现在就让我们解开它的神秘面纱！
ComputeThreeMaxima()
此函数的参数列表：直方图、直方图的bin长度和三个索引值

1. 定义了三个最大值，都是0
2. 按bin值进行遍历，获取每个bin内存放数的数量（bin代表的角度信息、存放的是关键点的索引），找出存放数数量前三位的bin
3. 如果第二大的远远小于（第一大的十分之一）第一大，索引2,3置-1
4. 如果第三大的远远小于（第一大的十分之一）第一大，索引3置-1
   所以回到前面，我们理解一下它是怎么剔除误匹配的呢
   以刚刚讲解的SearchByProtection()函数为例，首先我们确认直方图具体存放的是什么，初匹配完之后，它会计算他们之间的角度差异，为了能保证bin的范围，所以这边计算得到的角度会和一个规定的因子相乘，然后将匹配的bestindex放到相应的bin中，往下就是怎么剔除了。经过调用ComputeThreeMaxima()函数，我们将得到三个索引值，然后进行判断，这边的判断，其实就是少数服从多数，角度的偏差应该大家应该是差不多的，那些差异与大多数不一样的点都可能存在误匹配，所以都要剔除。

最后一个函数计算描述子距离
DescriptorDistance()
函数的参数列表：两个Mat矩阵，其实也是两个行向量。
这边应该是一个32维的描述子，现将第一行的数进行位异或运算，由于在描述子计算没有进行深度的理解，所以感觉这边的理解只能就看看代码，就不阐述了，后面补充学习描述子了再来补充吧。

这篇博客写了整整两天，1700行不到的代码，每天都近900行的代码阅读量，还是挺有成就感的，差不多就这样，那就明天见喽！

时间：2019年08月10日
作者：hhuchen
机构：河海大学机电学院