ORBSLAM2之LocalMapping线程

LocalMapping线程概述

  此线程主要用于处理Tracking线程发来的关键帧，更新关键帧和MapPoint之间的关系，对MapPoint进行检测。利用三角化生成更多的MapPoints。通过MapPoint融合加强MapPoint和帧之间的联系，清理冗余的的MapPoint。通过局部优化，优化局部关键帧pose和MapPoints。进一步提升MP和KF的质量。最后删除冗余关键帧，将剩下的关键帧送入回环检测模块。
流程如下：

一、 处理新关键帧ProcessNewKeyFrame()

1. 更新MapPoint属性
     a) 由Tracking线程可知关键帧特征点对应的MP一部分是跟踪而来，一部分是自己生成。跟踪而来的MP的属性在Tracking线程未更新过，在LocalMapping线程进行更新。
     b) 若当前关键帧某个特征点对应的MP为跟踪而来，更新该MP的属性，包括MP到帧、帧到MP，MP平均观测方向、描述子，观测范围。
     c) 若当前关键帧某个特征点对应的MP为自己生成，跟踪而来的MP存入 mlpRecentAddedMapPoints，等待MapPointCulling的检验。
2. 更新当前关键帧的共视图UpdateConnections()
     a) 遍历当前关键帧的MP，更新共视图
     b) 更新当前关键帧的子关键帧与父关键帧
       i. 与当前关键帧共视程度最高的帧作为当前关键帧的父关键帧
       ii. 当前关键帧设为父关键帧的子关键帧

二、 剔除质量不好的MapPoint MapPointCulling()

1. 检查当前关键帧生成的MapPoint和之前关键帧三角化恢复的一些MP作为最近添加的MP，这些MP都存于mlpRecentAddedMapPoints，作为待检MP
     i. 因为三角化恢复MP在MapPoint MapPointCulling()之后，所以当前帧三角化恢复出来的MP只能等到抽取到后面的关键帧才能进行检测。
2. 若待检MP被其他帧匹配的次数/观测到的次数<0.25，则置Bad，否则继续待检
     i. 被匹配到（Tracking中IncreaseFound），被观测到（Tracking中IncreaseVisible），
3. 若待检MP距离被创建经历了两个关键帧，然而被关键帧观测到的次数较少，则置bad，否则继续待检
4. 若待检MP已经经历了三个关键帧还没被置bad，则从待检队列中删除

三、 三角化创建新MapPoint CreateNewMapPoints()

若LocalMapping线程中待处理的关键帧都处理完了，则利用三角化恢复MP

1. 获取当前关键帧的共视关键帧
2. 计算当前关键帧与共视关键帧的基础矩阵
3. 极线约束搜索特征点SearchForTriangulation
     i. 遍历当前关键帧的特征点
     ii. 通过BoW加速当前关键帧特征点p1与共视关键帧的特征点p‘匹配
     iii. 匹配后进行极线检测
       1. p1*F12 为 pKF上的极线方程
       2. 检验匹配特征点p‘到极线的距离是否小于阈值从而判断是否匹配成功

    3. 根据旋转角剔除误匹配

4. 三角化恢复MapPoint
   i. 比较帧间视差角和双目视差角大小

    比较${\theta }_3$与$\max \left({\theta }_1,{\theta }_2\right)$关系
    若${\theta }_3>\max \left({\theta }_1,{\theta }_2\right)$，则利用两帧恢复MP
    若${\theta }_3<\max \left({\theta }_1,{\theta }_2\right)$，则利用1帧恢复MP
  ii. 利用线性三角化恢复特征点3D坐标，方法见直接线性变换法（DLT）在SLAM中的应用
  iii. 重投影检测MP坐标是否准确
  iv. 检测MP在两帧下的尺度是否连续

5. 更新
   i. 更新新生成的MP属性（建立两帧与MP之间的连接，描述子计算等等）
   ii. 更新mlpRecentAddedMapPoints，等待检验

四、 MapPoint融合

若LocalMapping队列中没有关键帧了，则进入该模式

1. 当前关键帧MP与共视关键帧MP相互融合
     i. 将当前关键帧MP与共视关键帧MP相互重投影进行匹配
     ii. 若匹配上的特征点有对应的MP则替换，若没有则添加

2. 更新当前关键帧的MP属性
     i. 平均观测方向、描述子、最大最远观测距离

3. 更新当前关键帧的共视图

五、局部BA

若LocalMapping队列中没有关键帧且回环线程没打断，则进入该模式

1. 该线程中局部BA与Tracking中不同，
     i. Tracking中只利用跟踪而来的MP找共视图进行BA
     ii. 此线程中是在MP融合后找到更多共视图后才进行BA
     iii. 该线程也优化MP，Tracking中是pose-only BA
2. 构造图模型
     i. 顶点： 1级邻接关键帧的MP+能观测到1级邻接关键帧MP的所有关键帧
     ii. 边：MP和pose之间添加边
3. 优化策略：
     i. 分几轮优化，每轮优化后利用卡方分布检查outlier，outlier不参与下次优化
     ii. 使用鲁棒核函数
4. 更新局部关键帧pose和1级邻接关键帧MP属性

六、删除冗余关键帧KeyFrameCulling(）

若LocalMapping队列中没有关键帧了，且BA优化完了，则进入该步骤

1. 遍历当前关键帧的1级邻接关键帧
2. 若1级邻接关键帧中90%以上的MP都能被其他至少3个关键帧观测到，则删除该关键帧

七、将当前关键帧插入闭环检测队列