ORB-SLAM2源码分析(单目)——局部建图线程
一、 局部建图线程简介
局部建图线程的主要作用:
(1) 承上启下。接收跟踪线程输入的关键帧并进行局部地图优化、删除冗余关键帧等;将优化后的关键帧发送给闭环线程。
(2) 实现中期数据关联。跟踪线程中仅使用了相邻普通帧或关键帧的信息,而且只优化当前帧的位姿,没有联合优化多个位姿,没有优化地图点。局部建图线程里满足一定共视关系的多个关键帧及其对应的地图点都参与优化。使得关键帧的位姿和地图点更加准确。
(3) 利用共视关键帧之间重新匹配得到更多新的地图点,增加地图里地图点的数目,可以提高跟踪的稳定性。
(4) 删除冗余关键帧可以降低局部BA的规模和次数。提高实时性。
跟踪线程和局部建图线程操作范围对比:
跟踪线程中,只跟上一帧的数据进行关联处理,而局部建图线程中,使用全部的关键帧进行关联处理
局部建图线程流程:
局部建图线程、跟踪线程、闭环线程是并行的关系
二、 删除冗余的地图点(MapPointCulling)和关键帧(KeyFrameCulling)
删除冗余的地图点(MapPointCulling):
(1) 根据相机类型设置不同的观测阈值
用于后续检测地图点的被观测次数,如果小于该阈值,则代表该地图点被关键帧看到的次数太少,则删除该地图点
(2) 遍历检查新添加的地图点,判断是否要删除地图点
删除冗余的关键帧(KeyFrameCulling):
要在处理完所有关键帧后,在进行删除冗余关键帧操作
检测当前关键帧在共视图中的关键帧,根据地图点在共视图中的冗余程度剔除该共视关键帧
冗余关键帧的判定:90%以上的地图点能被其他关键帧(至少3个)观测到
(1) 根据共视图提取当前关键帧的所有共视关键帧
(2) 对所有的共视关键帧进行遍历
(3) 遍历该共视关键帧的所有地图点,其中能被其它至少3个关键帧观测到的地图点为冗余地图点
(4) 如果该关键帧90%以上的有效地图点被判断为冗余的,则认为该关键帧是冗余的,需要删除该关键帧
三、 检测当前帧是否为关键帧(NeedNewKeyFrame)
判断当前帧是否为关键帧
(1) 纯VO模式和局部地图线程被占用情况下,不进行检测
检测成关键帧时,会将该关键帧插入到局部地图线程,如果局部地图线程正在被闭环检测使用,则不进行插入。
(2) 获取当前地图中的关键帧数目,如果距离上一次重定位较近,并且关键帧数目超出了最大限制,不插入关键帧
(3) 当前帧有多少地图点是被其他关键帧观测过
(4) 判断是否将当前帧当成关键帧进行插入
四、 关键帧之间生成新的地图点(CreateNewMapPoints)
当前关键帧与相邻关键帧通过三角化产生新的地图点,使得跟踪更稳
(1) 在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧
(2) 遍历相邻关键帧,搜索匹配并用极线约束剔除误匹配,最终三角化
(3) 判断相机运动的基线是不是足够长
(4) 根据两个关键帧的位姿计算它们之间的基础矩阵
(5) 通过词袋对两关键帧的未匹配的特征点快速匹配,用极线约束抑制离群点,生成新的匹配点对
(6) 对每对匹配通过三角化生成3D点,和 Triangulate函数差不多
· 取出匹配特征点
· 利用匹配点反投影得到视差角
· 对于双目,利用双目得到视差角;单目相机没有特殊操作
· 三角化恢复3D点
· 检测生成的3D点是否在相机前方,不在的话就放弃这个点
· 计算3D点在当前关键帧下的重投影误差
· 检查尺度连续性
· 三角化生成3D点成功,构造成MapPoint
· 为该MapPoint添加属性
· 将新产生的点放入检测队列
五、 融合重复的地图点(SearchInNeighbors)
要在处理完所有关键帧后,在进融合重复地图点操作
检查并融合当前关键帧与相邻帧(两级相邻)重复的地图点
(1) 获得当前关键帧在共视图中权重排名前nn的邻接关键帧
(2) 存储一级相邻关键帧及其二级相邻关键帧
(3) 将当前帧的地图点分别投影到两级相邻关键帧,寻找匹配点对应的地图点进行融合,称为正向投影融合
(4) 将两级相邻关键帧地图点分别投影到当前关键帧,寻找匹配点对应的地图点进行融合,称为反向投影融合
(5) 更新当前帧地图点的描述子、深度、平均观测方向等属性
(6) 更新当前帧与其它帧的共视连接关系
六、 局部线程BA(LocalBundleAdjustment)
(1) 将当前关键帧及其共视关键帧加入局部关键帧
(2) 遍历局部关键帧中(一级相连)关键帧,将它们观测的地图点加入到局部地图点
(3) 得到能被局部地图点观测到,但不属于局部关键帧的关键帧(二级相连),这些二级相连关键帧在局部BA优化时不优化
(4) 构造g2o优化器
(5) 添加待优化的位姿顶点:局部关键帧的位姿
(6) 添加不优化的位姿顶点:固定关键帧的位姿,注意这里调用了vSE3->setFixed(true)
不优化为啥也要添加?回答:为了增加约束信息
(7) 添加待优化的局部地图点顶点
(8) 在添加完了一个地图点之后, 对每一对关联的地图点和关键帧构建边
(9) 分成两个阶段开始优化。
(10) 检测outlier,并设置下次不优化
(11) 排除误差较大的outlier后再次优化 – 第二阶段优化
(12) 在优化后重新计算误差,剔除连接误差比较大的关键帧和地图点
(13) 优化后更新关键帧位姿以及地图点的位置、平均观测方向等属性