CL关于ORB_SLAM的那些事（LoopClosing）

LoopClosing部分的学习总结

今天来看闭环检测部分的代码，上一次把优化部分的代码都看了一遍，对于g2o方面的使用以及后端优化的图构建都有了一定的了解，今天来看闭环检测部分，来了解一下前端到底从啥时候开始触发闭环优化。

.h文件浏览

打开头文件，其中包括了
KeyFrame.h
LocalMap.h
Map.h
ORBVocabulary.h
Tracking.h
KeyFrameDatabase.h
还有g2o中的一些头文件

LoopClosing类中函数方法：
构造函数
设置跟踪者
设置局部地图
运行函数
插入关键帧
请求重启
运行全局BA
正在运行全局BA，这边是内联函数，直接返回运行的状态
已经完成全局BA，这边是内联函数，直接返回是否完成的状态
请求完成
已经完成
########我只是一根分割线，下面的函数是protected##########
校核新的关键帧
检测回环
计算Sim3
搜索和重用
修正闭环
如果要求就重启
校核完成
设置完成

LoopClosing类中维护的变量：
将存放关键帧的set与int的pair重命名为ConsistentGroup
将关键帧和Sim3的map重命名为KeyFrameAndPose，其中自定义了关键帧的比较函数、迭代器
protected的变量：
bool型的重启要求标志位
重启线程锁
bool型完成要求标志位
bool型完成的标志位
完成的线程锁
地图的指针
跟踪的指针
关键帧数据集的指针
ORBVocabulary的指针
局部地图的指针
存放关键帧指针的链表（闭环关键帧队列）
闭环队列的线程锁
Covisibility一致性的阈值
当前关键帧
匹配的关键帧
存放ConsistentGroup的容器
存放关键帧的两个容器（一个是足够一致性坐标系的关键帧、一个是当前帧连接的关键帧）
存放MapPoint的两个容器（一个是当前匹配的MapPoint、一个是闭环MapPoint）
从世界到当前的带尺度的变换矩阵
g2o内定义的Sim3
上一次闭环的关键帧帧号
运行全局BA的标志位
完成全局BA的标志位
停止全局BA的标志位
全局BA的线程锁
全局BA的线程指针
固定尺度的标志位
全BA的索引标志位

function by function

首先看一下其中包含的除了本身之外的头文件
Sim3Solver.h
Converter.h
Optimizer.h //昨天刚刚看过耶
ORBmatcher.h

构造函数
LoopClosing()
函数的参数列表：地图的指针、关键帧数据集的指针、ORB词汇的指针和固定尺度的标志位
列表初始化中，初始化了：
重启要求的标志位为false
完成的要求标志位为false
完成的标志位为true
将传入的地图指针赋值给维护变量中的地图指针
将传入的关键帧数据集指针赋值给维护变量中的关键帧数据集指针
将传入的ORB词汇指针赋值给维护变量中的ORB词汇指针
匹配的关键帧指针置空
上一次闭环的关键帧ID为0
正在运行全局BA的标志位为false
已经完成全局BA的标志位为true
停止全局BA的标志位为false
全局BA的线程指针置空
将传入的固定尺度标志位赋值给维护变量中的固定尺度的标志位
全BA的ID为0
函数内执行了Covisibility一致性阈值为3

设置跟踪指针
SetTracker()
函数的参数列表：Tracking的指针
将传入的Tracking指针赋值给维护变量的Tracking指针

设置局部地图
SetLocalMapper()
函数的参数列表：局部地图的指针
将传入的局部地图的指针赋值给维护变量的局部地图指针

运行函数
Run()
已经完成的标志位为false
while死循环，调用check在队列中是否有关键帧，如果有，进入检测回环的函数，如果检测到回环就计算Sim3，得到Sim3之后，调用修正回环的函数进行修正。如果没有在队列中没有检测到新的关键帧，调用如果请求就进行重启的函数，如果检测到完成了修正过程，直接跳出循环。没完成的话，就继续休眠5秒。跳出循环后调用设置完成的函数。

插入关键帧
InsertKeyFrame()
函数的参数列表：关键帧的指针
上锁回环队列
在当前帧不是第一帧的情况下，将此关键帧存入闭环关键帧队列

校核是否有新的关键帧
CheckNewKeyFrames()
上锁闭环关键帧队列
当队列中不为空时，直接返回true。

检测闭环
DetectLoop()
上锁闭环关键帧队列
取队列中的第一帧关键帧
将这一帧从队列里pop出来
将当前关键帧设置成不被移除（防止被这个线程处理的时候被删除）
如果当前帧的帧号少于（上一次闭环帧帧号+10），将此帧存入关键帧数据集中，将当前帧设置成可以移除的状态，最后直接返回false
获取按照权重排序的关键帧，存入连接的关键帧容器内（const）
取出当前帧的词汇向量（const）
循环遍历连接关键帧容器内的关键帧，判断该关键帧是否bad，不bad取出这个关键帧的词汇向量，调用ORB词汇类内的打分函数，计算这两帧之间的相似度，找出最低分。
调用关键帧数据集中的检测闭环坐标系函数，获取强制最低分的一些关键帧
如果在关键帧数据集中不存在闭环的候选关键帧，将这一帧关键帧添加进关键帧数据集，将一致性组清空，将当前帧设置成可移除，最后直接返回false。
将足够一致性的候选容器清空
定义存放当前一致性组的容器
定义bool型的容器（一致性组），容器的大小为维护变量中一致性组容器的大小，初始值为false
循环遍历候选关键帧容器中的关键帧，获取取其连接的关键帧，把这一帧关键帧插入连接关键帧的容器内，定义足够一致性的标志位为false，定义对一些组的一致性的标志位为false；循环遍历维护变量中的一致性组容器内的内容，先取整个set容器内的关键帧，定义一致性的标志位为false，循环取候选帧的连接关键帧容器的关键帧，如果之前一致性组内与候选帧连接关键帧组中交集有一帧，就将一致性标记位赋值true，在一些组内一致的标记位也设置成true，直接break。在一致性的标志位为true时，获取之前这组的一致性数量，在此基础上+1得到现在的一致性数量，在该一致性组还没存时（都初始化为false），将候选帧的一致性组与当前的一致性数量pair起来，存入当前一致性组内，并将一致性组对应的标记位置true。如果当前的一致性数量不小于阈值且足够的一致性的标记位为false，那么把当前的候选关键帧存入足够一致性的候选者的容器内并将足够一致性的标记位置true。对于没找到一致性组的组，就把这组关键帧和0 make pair，存入当前的一次性组。
将当前的一致性组赋值给维护变量中的一致性组
把当前帧存入关键帧数据集中
如果足够一致性候选容器为空，就把当前帧设置成了移除的状态，直接返回false；否则直接返回true。
最后将当前帧设置成可移除的状态
返回false。

计算Sim3
ComputeSim3()
获取足够一致性的候选帧的数量
定义了ORB匹配
定义了存放Sim3求解器的容器，容器的大小为候选帧的数量
定义了存放MapPoint容器的容器，容器的大小为候选帧的数量
定义了bool的容器，标记是否丢弃，容器的大小为候选帧的数量
定义候选者的数量为0
循环遍历所有的候选帧，将其设置成不可移除，如果该关键帧is bad，就在对应位置的丢弃容器内置true，直接continue；往下执行，将当前帧和该关键帧进行词袋匹配，如果匹配的点对数量少于20，丢弃容器对应位置置true，直接continue；否则，调用Sim3求解器函数，建立求解器；候选者数量++。
定义匹配的标志位并置false
当还有候选者且没找到最好的匹配，while循环，被丢弃的帧就不考虑，取出这一关键帧，定义存放bool的内点标志位容器，定义内点的数量，定义不再进行计算的标志位，取出Sim3的求解器，调用迭代函数，传入迭代次数5、不再计算的标志位、内点的标志位容器和内点的数量，计算得到Sim3。如果不再计算的标志位为true，该关键帧也将被丢弃，候选者数量–；如果计算得到了Sim3，定义存放MapPoint的容器，容器的大小为匹配上的MapPoint的数量，指针全部置空，循环遍历内点的标志位容器，如果是内点的话，就把这个MapPoint的指针存入刚刚定义容器内，从Sim3中获取旋转和平移矩阵，还有尺度因子，通过这些参数再对当前帧和该关键帧进行匹配，将Sim3转换成g2o内的Sim3，调用优化类内优化Sim3的函数，返回内点的数量。如果内点的数量不少于20，代表这两帧已经匹配上了，将匹配的标志位值true，将当前的关键帧赋值给匹配上的关键帧，获取该关键帧的Sim3，相乘得到当前帧相对于世界坐标系的Sim3，将匹配内点存入当前匹配的点的容器内，最后break，跳出循环。
如果循环下来，没找到能与当前帧匹配上的关键帧，就把那些候选帧直接从足够一致性候选人的容器内移除，再把当前帧设置成可移除，最后返回false。
获取被匹配上的关键帧的按照权重排序连接上的关键帧
将匹配上的关键帧存入闭环连接的关键帧容器内
将闭环MapPoint的容器清空
循环遍历与此关键帧连接的容器，取关键帧，获取此关键帧的MapPoint，遍历这些MapPoint，在是MapPoint且是good，另外此MapPoint的闭环点对应的关键帧的帧号不为当前帧，就把这个点存入闭环MapPoint的容器内，该帧号被赋值为当前帧的帧号。
通过上面计算得到的Sim3进行投影，得到更多的匹配关系
定义总共的匹配数量为0
循环遍历当前匹配上的MapPoint，计数
如果匹配上的总数量不少于40，遍历初始的候选者，不是那个被匹配上的关键帧都被设置成可移除，最后return true；否则，所有的候选者都被设置成可被移除，最后return false。

修正闭环
CorrectLoop()
屏幕上打印出“检测到回环”
调用局部地图内的要求停止，避免新的关键帧被插入
判断全局BA是否正在运行，如果是，将停止的标志位置true，全BA的索引++，如果全局BA的线程指针还有内存，调用线程指针中的detach()函数，最后释放线程指针指向的那块内存。
通过while循环，保证局部地图已经有效地停止了。
更新当前帧的连接关系
获取存放与当前帧连接的关键帧的容器
将该关键帧也存入这个容器内
用KeyFrameAndPose定义修正的Sim3、没修正的Sim3
将直接计算得到的Sim3存入当前帧的位置
获取当前帧的逆位姿 Twc
地图更新上锁
循环遍历连接的关键帧，获取其位姿，如果取到的关键帧不是当前帧，两个位姿相乘，得到两帧之间的变换矩阵，包括还有旋转、平移矩阵，先将其构建成Sim3，再通过g2oSic*mg2oScw得到修正之后的Sim3，将其存入修正过Sim3的容器内。再往下直接构建初步的Sim3矩阵，存入没修正的Sim3的容器内。（没修正中多了当前帧）
循环遍历修正过的Sim3的关键帧，取出关键帧、对应的Sim3（修正过和没修正的）和修正过的Sim3的逆，获取取出这帧的匹配MapPoint，循环遍历MapPoint，是MapPoint且不是bad且没被当前帧修正过，只有满足这些条件猜能往下执行，不然直接continue。获取这个点的世界坐标，将这个坐标转换成3D向量，将这个坐标通过没修正的Sim3投影，再经过修正过的Sim3进行投影得到修正过的3D坐标，将其转换成Mat格式，重新设置这个MapPoint的Pose，对应的修正的帧号就是当前关键帧的帧号，修正的参考帧就是取出来的这帧帧号，更新这个MapPoint的法向量和深度值。取出当前取出的关键帧的Sim3的旋转矩阵并将其，转换成旋转矩阵，取出平移部分转换成3D向量，取出尺度值，平移向量除以这个尺度，组合形成修正的变换矩阵，设置当前取出的这个关键帧的位姿，更新连接关系。
循环遍历当前匹配的MapPoint，取出其中的MapPoint，取出当前帧中对应索引的MapPoint，如果当前帧的MapPoint存在，就把这个MapPoint换成闭环时当前匹配的MapPoint。如果当前帧的MapPoint不存在，就把这个MapPoint添加入当前关键帧，该MapPoint也添加当前关键帧的观测，计算这个MapPoint的描述子。
调用SearchAndFuse()函数，传入修正过的Sim3容器。
定义关键帧和set<关键帧>之间的map，闭环连接关系
循环遍历连接上的关键帧，分别取其中的关键帧，获取与其连接的关键帧，更新取出这帧关键帧的连接，在此帧处取出与此帧连接的set内的关键帧，循环遍历取出来的关键帧的Covisible关键帧，在闭环连接的容器内，在该关键帧的索引位置删除这些set内有的关键帧，循环遍历当前连接的关键帧，同样也是在该关键帧的索引位置删除这些set内有的关键帧。
调用优化类中的OptimizeEssentialGraph()函数
调用地图指针的通知有新的大变换函数InformNewBigChange()
匹配上的关键帧添加闭环边（当前关键帧）
当前关键帧添加闭环边（匹配上的关键帧）
将运行全局BA的标志位置true
将已完成全局BA的标志位置false
将停止全局BA的标志位置false
启动一个新的线程，运行全局BA
释放局部地图指针
将上一次闭环的关键帧ID赋值为当前帧的帧号

寻找和重用
SearchAndFuse()
函数的参数列表：KeyFrameAndPose的引用
定义ORB匹配的对象
循环遍历传入的修正位姿地图容器，取first的关键帧，取second的对应Sim3，利用转换类的函数将g2o类型转换成Mat，定义存放MapPoint的容器，容器的大小为闭环内MapPoint的数量，初值为空，调用匹配的Fuse函数，地图更新上锁，获取闭环MapPoint的数量，循环更新、替换地图中的MapPoint。

要求重启
RequestReset()
重启要求的标志位置true
while（1）死循环，死休眠，除非要求重启的标志位置false

运行全局BA
RunGlobalBundleAdjustment()
函数的参数列表：闭环关键帧的帧号
屏幕上打印“开始全局BA”
定义索引值，赋值为全BA索引
调用优化类中的全局BA函数
如果索引值不等于全BA索引，直接return
如果停止全局BA的标志位为false，屏幕上打印“全局BA已完成”，换行打印“正在更新地图…”，局部地图指针发出请求停止，while（局部地图没被停止且局部地图没有完成），陷入死睡眠。定义存储关键帧的list，先存入地图中的所有关键帧，循环遍历list中的关键帧，取出list中的font关键帧，获取其子关键帧，获取其逆位姿，循环子关键帧，取其中的子关键帧，判断其为是否为当前关键帧全局BA过，如果不是，获取其位姿变换乘上上面关键帧的逆位姿，得到关键帧到子关键帧的变换矩阵，再乘上关键帧全局BA之后的变换矩阵得到子关键帧的全局BA变换矩阵，子关键帧的全局BA优化的索引关键帧为当前闭环关键帧的帧号。在关键帧中更新全局BA之前的变换矩阵和全局BA之后的变换矩阵，将此关键帧pop出去。
获取地图中的所有MapPoint
循环遍历MapPoint，跳出bad的MapPoint，对于已经被全局BA优化的坐标，直接将优化过的坐标更新；对于没有被优化过的坐标，获取该MapPoint的参考关键帧，如果该参考关键帧也没被全局BA优化过，那就直接continue下一个点；获取参考关键帧的旋转和平移矩阵（没优化之前的），通过这两个矩阵得到该点在该关键帧中的三维坐标，再利用优化完的变换矩阵，重投影得到新的世界坐标，直接进行MapPoint坐标的更新。
地图通知有新的大变化
释放局部地图
屏幕上打印“地图更新完”
完成全局BA标志位置true
运行全局BA标志位置false

要求完成
RequestFinish()
将完成要求的标志位置true

校核完成
CheckFinish()
返回完成要求的标志位状态

设置完成
SetFinish()
将完成的标志位置true

已经完成
isFinished()
返回已经完成的标志位

LoopClosing部分的代码算是看完了，大致整理了一下闭环检测部分的过程，如图所示

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