09 闭环线程 LoopClosing

09 闭环线程 LoopClosing

9.1 主函数：Run()

成员函数/变量	访问控制	意义
CheckNewKeyFrames()	protected	查看队列中是否有未处理的关键帧
DetectLoop()	protected	检测是否有回环
ComputeSim3()	protected	计算当前关键帧和闭环关键帧之间的相似变换矩阵
CorrectLoop()	protected	回环矫正

主要流程

9.1.1 闭环检测

LoopClosing 类中定义了 ConsistentGroup 类型，表示关键帧组，

typedef pair<set<KeyFrame *>, int> ConsistentGroup

• 第一个元素表示一组共视关键帧；
• 第二个元素表示该关键帧组的连续长度。

成员函数/变量	访问控制	意义
KeyFrame *mpCurrentKF	protected	当前关键帧
KeyFrame *mpMatchedKF	protected	当前关键帧的闭环匹配关键帧
std::vector<ConsistentGroup> mvConsistentGroups	protected	前一关键帧的闭环候选关键帧组
vCurrentConsistentGroups	protected	当前关键帧的闭环候选关键帧组
std::vector<KeyFrame *> mvpEnoughConsistentCandidates	protected	所有达到足够连续数的关键帧

闭环检测原理: 若连续 4 个关键帧都能在数据库中找到对应的闭环匹配关键帧组，且这些闭环匹配关键帧组间是连续的，则认为实现闭环。

具体步骤如下：

① 找到当前关键帧 mpCurrentKF 的闭环候选关键帧 vpCandidateKFs，闭环候选关键帧取自于 与当前关键帧有相同 BOW 向量，但不存在直接连接的关键帧。

② 将闭环候选关键帧和其共视关键帧组合成为关键帧组 vCurrentConsistentGroups

③ 在当前关键组和前一帧的连续关键组间寻找连续关系，关键帧组的连续关系是指两个关键帧组间是否有关键帧同时存在于两关键帧组中。

• 若当前关键帧组在之前的连续关键帧组中找到连续关系，则当前的连续关键帧组的连续长度加1；

• 若当前关键帧组在之前的连续关键帧组中没能找到连续关系，则当前关键帧组的连续长度为0。

若某关键帧组的连续长度达到3，则认为该关键帧实现闭环。

9.1.2 计算Sim3变换（相似变换）: ComputeSim3()

Sim3 变换即相似变换，包含旋转、平移和尺度因子。当机器人绕了一圈后，累积误差会很大，可能产生漂移。对于单目相机来说，深度信息是需要考虑的，也就是尺度因子；对于双目相机和 RGBD 相机，通常认为没有尺度漂移。

由于累计误差的存在，检测到回环后，当前关键帧和闭环关键帧的位姿并不是相等的，这时就需要计算二者之间的相似变换矩阵，用于后续优化。

成员函数/变量	访问控制	意义
std::vector<KeyFrame *> mvpEnoughConsistentCandidates	protected	在函数LoopClosing::DetectLoop()中找到的有足够连续性的闭环关键帧
g2o::Sim3 mg2oScw
cv::Mat mScw	protected	世界坐标系w到相机坐标系c的Sim3变换
std::vector<MapPoint *> mvpLoopMapPoints	protected	闭环关键帧组中的地图点
std::vector<MapPoint *> mvpCurrentMatchedPoints	protected	当前帧到 mvpLoopMapPoints 的匹配关系 mvpCurrentMatchedPoints[i] 表示当前帧第 i 个特征点对应的地图点

9.1.4 闭环矫正: CorrectLoop()

函数 LoopClosing::CorrectLoop() 的主要流程：

① Sim3 位姿传播:

• 将Sim3位姿传播到局部关键帧组上
• 将Sim3位姿传播到局部地图点上

② 地图点融合:

• 将闭环关键帧组地图点投影到当前关键帧上
• 将闭环关键帧组地图点投影到局部关键帧组上

③ BA优化：

• 本质图BA优化: 优化所有地图点和关键帧位姿,基于本质图
• 全局BA优化: 优化所有地图点和关键帧位姿,基于地图点到关键帧的投影关系