这篇文章将向大家介绍LoopClosing模块中的地图回环矫正和地图合并和矫正部分的内容，其函数分别为CorrectLoop和MergeLocal（或MergeLocal2），其根据从回环检测过程中求解出的回环匹配关键帧或合并匹配关键帧到当前帧的Sim(3)变换进行回环和地图合并及矫正。
关于回环矫正的几个问题：
问题1： 在ORB-SLAM3中CorrectLoop的函数[1]的功能中根据前面优化求得的变换mg2oLoopScw应用到当前关键帧及其共视帧（统称为连接帧mvpCurrentConnectedKFs）上，然后进行OptimizeEssentialGraph进行优化，请详细分析一下具体的步骤及思路。
回答：mg2oLoopScw是 Sim3Solver计算得到的回环帧到当前帧的 Sim(3) 变换所得矫正后位姿，用于修正关键帧及其连接帧集合（当前关键帧的所有共视关键帧）的位姿（将该位姿和调整关键帧和当前关键帧的相对变换进行叠加及可以对每一个共视关键帧的位姿进行初步矫正），使整个局部关键帧集合做统一一致的初步矫正。然后调用OptimizeEssentialGraph进行通常在闭环检测后调整关键帧的位姿，保持整体一致性。这里使用sim(3)变换，因为闭环可能涉及尺度漂移，需要调整尺度。(而GlobalBundleAdjustment是全局BA，优化所有关键帧和地图点的位置，使用se(3)因为BA通常处理的是刚体变换，不考虑尺度，将回环约束均匀传播到整个关键帧图)。关于优化部分的内容，将会在专门的文章中加以介绍。关于回环矫正的更详细的思路，可以参考本文后面地图合并的部分内容。
问题2：首先将当前关键帧及其共视关键帧应用一致的矫正变换然后再进行全局优化。这个步骤不添加是否会有影响？
回答：一般的情况是回环匹配帧由于在地图的时间点前面，其误差和尺度漂移的程度要小，当前关键帧和回环匹配帧之间也许走过不少的其他关键帧，积累的较多的误差，因此首先通过将当前关键帧的共视关键帧首先应用统一的矫正变换以实现初步的累积误差和尺度漂移矫正，这对于后面的全局优化的过程能够更加容易收敛。
关于地图合并的几个问题：
问题1：地图合并的场景有没有比较具体的应用场景的示例帮助理解？
回答：ORB-SLAM3支持多地图(multi-map，由Atlas类来管理地图集)，在地图合并和回环状态的检测的过程中不同的地方在于匹配关键帧和当前关键帧在不在同一个地图里。比如家庭里的多个房间，工厂里的多个车间等。
问题2：地图合并时的实现逻辑是怎样的？
回答: 地图合并时主要采取如下的步骤：（1）、首先根据当前帧来更新一定窗口大小的共视帧数量（如果当前关键帧的直接共视关键帧的数量不够，则继续在共视帧的共视帧中去扩展），同理也对合并匹配帧去获取其共视帧的集合。（2）、然后对当前关键帧窗口内的关键帧采用和回环纠正第一步的Sim(3)变换一样的操作实现初步的位姿矫正以便于后续的全局优化的迭代收敛。（3）、执行地图合并操作，将当前帧相关的关键帧（spLocalWindowKFs变量里的当前这的局部窗口内的帧的集合，包括前后一段时间内的连续关键帧和共视关键帧等）合并到融合的地图里pMergeMap->AddKeyFrame(pKFi)，并从当前的地图里移除对应的关键帧pCurrentMap->EraseKeyFrame(pKFi)。地图点的合并过程类似。然后Atlas地图集管理将融合地图设为当前地图（表示当前系统已经实现了地图跳转），并将当前地图标记为失效地图；（4）、将当前关键帧的父关键帧设为合并匹配关键帧，并逆序当前关键帧的父子关系链条；（5）、在合并后有些地图点存在着重复（比如合并匹配帧及附近的共视关键帧和当前关键帧及附近的共视关键帧有些地图点是重复的），通过sim(3)变换后去搜索这些重复的地图点并剔除冗余（调用SearchAndFuse(vCorrectedSim3, vpCheckFuseMapPoint)函数），针对每一个当前关键帧邻近窗口内的关键帧集合和融合关键帧相连接的关键帧集合的每一个关键帧调用UpdateConnections更新相关连接信息；（6）调用函数Optimizer::LocalBundleAdjustment(mpCurrentKF, vpLocalCurrentWindowKFs, vpMergeConnectedKFs,&bStop)进行局部优化，其中vpMergeConnectedKFs参数中的关键帧的位姿为fixed不参与优化（与优化的关键帧共享的地图点会参与优化，但也会受约束于固定的位姿，这给优化的过程提供了基准参考，防止优化出现漂移现象）。（7）对当前地图的没有融合到pMergeMap部分的关键帧同样采用Sim(3)矫正变换以保证整个地图的一致性，并再一次通过加锁机制实现地图的合并操作（相关关键帧和地图点在地图集里的关系更新，在代码的实现上和第三步的合并操作一样）（8）调用全局线束优化，对合并后的当前地图的所有关键帧位姿和地图点集合进行联合优化，以消除较长时间运行的累积误差，在优化前会让局部建图器停止工作。
问题3：地图合并的时候，将当前地图设置为失效，下次合并的时候也可能会再次变为当前地图对吗？
回答：是的，在 ORB-SLAM3 中，地图合并时将当前地图设置为“失效”（即 SetMapBad()），目的是标记当前地图已经不再使用，并且可能在之后的合并过程中被再次激活作为当前地图。
问题4：由于在地图合并的时候存在着关键帧父子关系链的调整，会出现两个子问题：（1）、将当前关键帧的父关键这设为融合匹配关键帧，并调整当前关键帧的父子关键帧链条上的顺序结构，则之前的融合匹配关键帧的子关键帧链条如何维护？(2)、关键帧之间的父子关系链和关键帧之间的前后顺序双向链表定义是不同的数据结构意义，双向链表主要体现在时间上的先后顺序，父子关系主要体现在共视的重合程度吗？
回答：（1）、在支持多地图的合并场景下，地图的合并将会形成在合并点出现树的分叉结构（每次融合可以理解将融合关键帧的现有子关键帧链保持，同时添加一个当前关键帧的子关键帧链条），这种分叉结构可以保证父子关键帧关系图不会出现闭环形结构。（2）、父子关键帧在一般情况下保持时序（一般在mbFirstConnection为true时建立父子关系），且主要体现了关键帧之间的共视重合度，但在上述的合并地图的场景下，为了优化地图结构，会调整关键帧之间的父子关系结构，此时部分父子关系链上的结构将不再保证时序关系。关键帧里的双向链表结构确实保持了关键帧的时序关系。可以方便获取当前帧在时间序列上的前一个和后一个关键帧。
问题5：地图合并时由于每个地图的坐标系独立，即一般在跟踪失效的时候会创建新地图，新地图的初始关键帧的坐标为标准的单位矩阵？在两个地图的融合时坐标如何对齐？和回环检测和纠正时的处理有什么具体的差异？
回答：一般在回环检测和矫正以及地图合并检测和合并的处理过程中，会以先前的回环匹配帧或合并匹配帧作为基准（后面经过了一段时间的累积出现了累积的漂移误差），基于两个匹配帧的变换将当前帧及当前地图的所有帧都首先进行Sim(3)的变换进行初步纠正，这个变换逻辑是一样的。
References
- [1]、CorrectLoop函数实现: LoopClosing.cc#L1028
Leave a Reply