loopclosing线程中处理的kf是由localmapping线程送进来的。
步骤1:检测是否有kf送入,如果有则进行下面的步骤,如果没有就不处理。
步骤2:检测是否出现回环。
2.1如果距离上次loopclosing还没有超过10个kf,或者loopclosing中还没有10个kf则不进行回环检测。
2.2计算当前kf和相邻kf的bow得分,得出最低得分minScore
2.3在所有kf中找出备选关键,这些关键帧是和当前关键帧有共识的关键帧但不是当前关键帧的相邻关键帧,通过分组剔除孤立的候选关键帧。
2.4在所有候选关键帧中找出具有连续性的关键帧,也就是:备选帧中有连续三帧与当前帧的bow得分较高,才是可靠的闭环(应该是这个意思)。
见图:
步骤3: 计算用三对匹配点计算回环的相似变换矩阵(sim3)。
参考本文和原始论文。
步骤4 : Perform loop fusion and pose graph optimization(没太仔细看,先把吴博的原话抄上)
1通过求解的Sim3以及相对姿态关系,调整与当前帧相连的关键帧位姿以及这些关键帧观测到的MapPoints的位置(相连关键帧—当前帧)
2将闭环帧以及闭环帧相连的关键帧的MapPoints和与当前帧相连的关键帧的点进行匹配(相连关键帧+当前帧—闭环帧+相连关键帧)
3 通过MapPoints的匹配关系更新这些帧之间的连接关系,即更新covisibility graph
4对Essential Graph(Pose Graph)进行优化,MapPoints的位置则根据优化后的位姿做相对应的调整
5创建线程进行全局Bundle Adjustment