目录
LocalMapping线程概述
此线程主要用于处理Tracking线程发来的关键帧,更新关键帧和MapPoint之间的关系,对MapPoint进行检测。利用三角化生成更多的MapPoints。通过MapPoint融合加强MapPoint和帧之间的联系,清理冗余的的MapPoint。通过局部优化,优化局部关键帧pose和MapPoints。进一步提升MP和KF的质量。最后删除冗余关键帧,将剩下的关键帧送入回环检测模块。
流程如下:
一、 处理新关键帧ProcessNewKeyFrame()
- 更新MapPoint属性
a) 由Tracking线程可知关键帧特征点对应的MP一部分是跟踪而来,一部分是自己生成。跟踪而来的MP的属性在Tracking线程未更新过,在LocalMapping线程进行更新。
b) 若当前关键帧某个特征点对应的MP为跟踪而来,更新该MP的属性,包括MP到帧、帧到MP,MP平均观测方向、描述子,观测范围。
c) 若当前关键帧某个特征点对应的MP为自己生成,跟踪而来的MP存入 mlpRecentAddedMapPoints,等待MapPointCulling的检验。 - 更新当前关键帧的共视图UpdateConnections()
a) 遍历当前关键帧的MP,更新共视图
b) 更新当前关键帧的子关键帧与父关键帧
i. 与当前关键帧共视程度最高的帧作为当前关键帧的父关键帧
ii. 当前关键帧设为父关键帧的子关键帧
二、 剔除质量不好的MapPoint MapPointCulling()
- 检查当前关键帧生成的MapPoint和之前关键帧三角化恢复的一些MP作为最近添加的MP,这些MP都存于mlpRecentAddedMapPoints,作为待检MP
i. 因为三角化恢复MP在MapPoint MapPointCulling()之后,所以当前帧三角化恢复出来的MP只能等到抽取到后面的关键帧才能进行检测。 - 若待检MP被其他帧匹配的次数/观测到的次数<0.25,则置Bad,否则继续待检
i. 被匹配到(Tracking中IncreaseFound),被观测到(Tracking中IncreaseVisible), - 若待检MP距离被创建经历了两个关键帧,然而被关键帧观测到的次数较少,则置bad,否则继续待检
- 若待检MP已经经历了三个关键帧还没被置bad,则从待检队列中删除
三、 三角化创建新MapPoint CreateNewMapPoints()
若LocalMapping线程中待处理的关键帧都处理完了,则利用三角化恢复MP
- 获取当前关键帧的共视关键帧
- 计算当前关键帧与共视关键帧的基础矩阵
- 极线约束搜索特征点SearchForTriangulation
i. 遍历当前关键帧的特征点
ii. 通过BoW加速当前关键帧特征点p1与共视关键帧的特征点p‘匹配
iii. 匹配后进行极线检测
1. p1*F12 为 pKF上的极线方程
2. 检验匹配特征点p‘到极线的距离是否小于阈值从而判断是否匹配成功
3. 根据旋转角剔除误匹配
- 三角化恢复MapPoint
i. 比较帧间视差角和双目视差角大小
比较
θ
3
\theta_{3}
θ3与
max
(
θ
1
,
θ
2
)
\max \left(\theta_{1}, \theta_{2}\right)
max(θ1,θ2)关系
若
θ
3
>
max
(
θ
1
,
θ
2
)
\theta_{3}>\max \left(\theta_{1}, \theta_{2}\right)
θ3>max(θ1,θ2),则利用两帧恢复MP
若
θ
3
<
max
(
θ
1
,
θ
2
)
\theta_{3}<\max \left(\theta_{1}, \theta_{2}\right)
θ3<max(θ1,θ2),则利用1帧恢复MP
ii. 利用线性三角化恢复特征点3D坐标,方法见直接线性变换法(DLT)在SLAM中的应用
iii. 重投影检测MP坐标是否准确
iv. 检测MP在两帧下的尺度是否连续
- 更新
i. 更新新生成的MP属性(建立两帧与MP之间的连接,描述子计算等等)
ii. 更新mlpRecentAddedMapPoints,等待检验
四、 MapPoint融合
若LocalMapping队列中没有关键帧了,则进入该模式
-
当前关键帧MP与共视关键帧MP相互融合
i. 将当前关键帧MP与共视关键帧MP相互重投影进行匹配
ii. 若匹配上的特征点有对应的MP则替换,若没有则添加 -
更新当前关键帧的MP属性
i. 平均观测方向、描述子、最大最远观测距离 -
更新当前关键帧的共视图
五、局部BA
若LocalMapping队列中没有关键帧且回环线程没打断,则进入该模式
- 该线程中局部BA与Tracking中不同,
i. Tracking中只利用跟踪而来的MP找共视图进行BA
ii. 此线程中是在MP融合后找到更多共视图后才进行BA
iii. 该线程也优化MP,Tracking中是pose-only BA - 构造图模型
i. 顶点: 1级邻接关键帧的MP+能观测到1级邻接关键帧MP的所有关键帧
ii. 边:MP和pose之间添加边 - 优化策略:
i. 分几轮优化,每轮优化后利用卡方分布检查outlier,outlier不参与下次优化
ii. 使用鲁棒核函数 - 更新局部关键帧pose和1级邻接关键帧MP属性
六、删除冗余关键帧KeyFrameCulling()
若LocalMapping队列中没有关键帧了,且BA优化完了,则进入该步骤
- 遍历当前关键帧的1级邻接关键帧
- 若1级邻接关键帧中90%以上的MP都能被其他至少3个关键帧观测到,则删除该关键帧