当前位置：首页 > news >正文

orbslam2代码解读(4):loopclosing回环检测线程

news 来源：原创 2024/7/3 1:50:15

书接上回，介绍完了局部建图线程，局部建图线程在进行局部BA之后，也会将新的关键帧mpLoopCloser放进回环线程的mlpLoopKeyFrameQueue容器中。所以这时候回环检测线程就根据这个新的关键帧来进行回环检测的操作。

回环检测的主要程序

// 线程主循环while(1){// Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的// 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue// Step 1 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来if(CheckNewKeyFrames()){// Detect loop candidates and check covisibility consistencyif(DetectLoop()){// Compute similarity transformation [sR|t]// In the stereo/RGBD case s=1if(ComputeSim3()){// Perform loop fusion and pose graph optimizationCorrectLoop();}}}// 查看是否有外部线程请求复位当前线程ResetIfRequested();// 查看外部线程是否有终止当前线程的请求,如果有的话就跳出这个线程的主函数的主循环if(CheckFinish())break;//usleep(5000);std::this_thread::sleep_for(std::chrono::milliseconds(5));}// 运行到这里说明有外部线程请求终止当前线程,在这个函数中执行终止当前线程的一些操作SetFinish();

cvlife的注释版删掉一些冗余的解释后，主要就是这么一个流程，后续就逐个流程介绍。

step1，DetectLoop()

如果mlpLoopKeyFrameQueue容器中有新的关键帧，就会执行DetectLoop()函数。
这个函数的过程如下：

从队列中取出新的关键帧，作为当前检测闭环关键帧，且设置当前关键帧不要在优化的过程中被删除（mpCurrentKF->SetNotErase()）
如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
遍历当前回环关键帧所有连接关键帧（一级相邻关键帧），计算当前关键帧与每个共视关键帧的bow相似度得分，并得到最低得分minScore。意义：本质就是设置一个阈值，意思就是如果回环候选帧的相似度得分都比一级相邻关键帧的得分低，那么这些回环候选帧就不可信
在所有关键帧中找出相似度得分大于minScore的闭环候选帧（注意候选帧应该选不和当前帧相连的关键帧）。
在候选帧中检测具有连续性的候选帧。其实就是当一个候选关键帧符合相似度要求的时候，先不急着确立回环关系，仅仅当候选关键帧连续与三个（代码上写的是3个，也可以改）新的关键帧都能连接上的时候，才确立回环关系。
将当前新的关键帧放到关键帧数据库mpKeyFrameDB中，如果成功检测到回环就返回true，否则返回false。

step2，ComputeSim3()

如果上一个函数检测到回环了，那么就需要求出回环两个关键帧之间的匹配关系。如果是单目情况就需要考虑尺度漂移的问题，所以计算Sim3变换；如果是双目或者rgbd，这个尺度直接就设置成1。
这个函数的过程如下：

遍历闭环候选帧集，初步筛选出与当前关键帧的匹配特征点数大于20的候选帧集合（经典使用词袋找到两帧的匹配点对SearchByBoW），并为每一个候选帧构造一个Sim3Solver。
对每一个候选帧用Sim3Solver迭代匹配，直到有一个候选帧匹配成功，或者全部失败。匹配成功的过程：①先是通过Sim3Solver进行迭代求解，如果能够得到一个粗略的Sim3变换，那么就需要进一步优化这个粗略的Sim3变换。②通过matcher.SearchBySim3找到两帧更多的匹配点对（只有互相投影都成功匹配的才认为是可靠的匹配）。③用新的匹配点对通过g2o来优化Sim3（只优化Sim3，不优化地图点）。

SearchBySim3 g2o的Sim3优化
如果Sim3优化成功，取出闭环候选的关键帧及其共视关键帧，以及这些共视关键帧的地图点。
根据Sim3变换，将闭环候选关键帧及其连接关键帧的所有地图点投影到当前关键帧进行投影匹配(matcher.SearchByProjection)，如果匹配点对超过40个，则说明这个闭环是稳定且成功的，否则说明这个闭环是失败的，删除相关信息。

闭环候选帧往当前帧的投影过程

step3，CorrectLoop()

这个函数就是根据上一个函数计算出来的Sim3变换，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的地图点位置。（简单来说就是位姿和尺度的联合矫正）
这个函数的过程如下：

结束局部地图线程、全局BA，为闭环矫正做准备
根据共视关系更新当前关键帧与其它关键帧之间的连接关系（因为之前闭环检测、计算Sim3中增加了该关键帧的地图点，所以需要更新）
得到Sim3优化后，当前帧与世界坐标系之间的Sim变换在ComputeSim3函数中已经确定并优化，通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换，进行后续的矫正。
得到矫正的当前关键帧的共视关键帧位姿后，修正这些共视关键帧的地图点。修正的具体流程：①将当前kf及其共视kf的地图点进行遍历，先将地图点从世界坐标系根据初始的相机位姿(g2oSiw，这个相机位姿因为累计误差会有尺度漂移，所以实际上这个位姿是有尺度的)变换到相机坐标系。②再通过矫正后的相机位姿(g2oCorrectedSwi，这个Sim的尺度其实就是为了抵消尺度漂移而产生的尺度)将相机坐标系的地图点矫正回到世界坐标系当中。③至此这些地图点的尺度一致了。
根据（当前关键帧和其相连的关键帧）与世界坐标系之间的Sim3变换，将Sim3转换成SE3，并且更新关键帧的位姿。这时（当前关键帧和其相连的关键帧）与产生时间较早的闭环候选关键帧的位姿也尺度一致了。具体Sim3转换SE3的代码：

    // 调用toRotationMatrix 可以自动归一化旋转矩阵Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix(); Eigen::Vector3d eigt = g2oCorrectedSiw.translation();                  double s = g2oCorrectedSiw.scale();// 平移向量中包含有尺度信息，还需要用尺度归一化eigt *=(1./s);

最终当前关键帧和其相连的关键帧直接设置为校正后的SE3，且更新关键帧之间的连接关系。

检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补。本质上更相信回环过程中得到的地图点（时间靠前，累计误差小），而当前关键帧原来的地图点时间靠后可能有累计误差。
将闭环候选帧的相连关键帧组投影到当前关键帧的相连关键帧组中进行匹配，新增或替换当前关键帧的相连关键帧组中的地图点。跟第6步本质上是一样的，更相信闭环候选帧的相连关键帧组中的地图点，误差更小。第6、7步都是做关键帧之间地图点的更新融合，因为前面经过了尺度对齐，所以地图点肯定会有冲突。具体两个相连组也可以看下图：
更新当前关键帧组之间的两级共视相连关系，得到因闭环时地图点融合而新得到的连接关系。这个步骤最大的作用就是得到因为闭环地图点调整而新生成的连接关系，后续本质图优化的时候需要加的边。
进行本质图优化。仅仅优化本质图中所有关键帧的位姿(Sim3位姿优化)，不优化地图点。共视图比较稠密，本质图比共视图更稀疏，这是因为本质图的作用是用在闭环矫正时，用相似变换来矫正尺度漂移，把闭环误差均摊在本质图中。本质图中节点也是所有关键帧，但是连接边更少，只保留了联系紧密的边来使得结果更精确。本质图中的边包含：①生成树连接关系。②形成闭环的连接关系，闭环后地图点变动后新增加的连接关系。③当前帧与闭环匹配帧之间的连接关系(这里面也包括了当前遍历到的这个关键帧之前曾经存在过的回环边)。④共视程度超过100的关键帧也作为边进行优化。

本质图生成树

最后就是将优化后的位姿更新到关键帧中，并且地图点根据参考帧优化前后的相对关系调整自己的位置，然后更新地图点的平均观测方向和深度。

本质图将位姿优化完后，新建一个线程用于全局BA优化。因为在全局BA之前，数据已经做了很多处理，通过本质图对相机位姿进行了优化，并且地图点也根据参考帧优化前后的相对关系调整自己的位置，所以这个全局BA的代码还是比较容易懂的。需要注意的点是：①这个全局BA除了初始关键帧是固定的，其他关键帧和地图点都是需要优化的量。②相机位姿是SE3优化，因为在本质图优化的时候是Sim3优化，并且做了尺度归一化了，所以全局BA用的是SE3优化。

总结

完结撒花！！！这一系列的记录文章，主要是把orbslam2中一些主要的函数实现过程弄清楚，并且明白整个orbslam2的运行流程。所以不会对函数中具体的代码做详细的讲解（这部分完全可以去看代码注释），而我们要做的是明白orbslam2的流程和处理思路。在学习orbslam2的时候真的学习到了很多，之前看完视觉slam14讲，里面的内容确实很多很详细可以作为一个教科书，但是更多的还是得学优秀的开源项目，这样才能对slam整个框架有更深的理解。感谢orbslam2的讲解课程和注解代码。