tracking线程
- ORB 特征点提取
- 相机初始化
- 根据前一帧图片估计出当前帧的位姿
- 基于当前帧的位姿构建出局部地图 局部地图的作用是优化当前帧的位姿
- 决定是否生成关键帧
单目相机初始化
单目相机初始化成功的条件:连续两帧间成功三角化超过100个点,则初始化成功
初始化帧即为参考帧 第二帧的特征点个数要大与100
单目相机初始化步骤:
- 若单目相机初始化器还没创建,则创建初始化器
- 若上一帧特征点数量足够但是当前帧的特征点数量过少 则匹配失败 删除初始化器
- 在初始化帧和当前帧进行特征匹配
- 若匹配的特征点数目太少,则匹配失败,删除初始化器
- 进行单目初始化
- 单目初始化成功后,删除未成功三角化的匹配点对
- 创建初始化地图 以初始化帧为参考系
创建好初始化地图之后:
- 计算两个关键帧的词袋
- 将两个关键帧插入地图
- 处理所有的地图点
- 全局BA 优化所有关键点位姿和地图点
- 将两帧的平移尺度归一化
- 将坐标点尺度也归一化
- 将关键帧插入地图 更新归一化后的地图点坐标和位姿
- 更新状态跟踪向量
初始位姿估计
根据恒速运动模型估计位姿
根据参考帧估计位姿
通过重定位估计位姿
步骤:
- 检查是否初始化
- 检查恒速运动莫模型估计是否成功
- 若没成功则使用参考帧估计位姿
- 若还没成功则使用重定位估计位姿
- 若还没成功则标记跟踪lose
使用双目相机或者RGBD相机时会生成大量临时地图点 若临时地图点太多或者深度太大 则停止生成临时地图点 临时地图点的作用增强匹配 临时地图点在跟踪成功后就会被删掉
如果跟踪失败了不会产生新的关键帧 这些1地图点不会被注册进地图 不会对之后的建图造成影响
恒速运动模型:
假设模型为恒速运动 进行特征点匹配 若匹配的特征点超过十个 则认为时恒速运动模型
根据参考帧位姿估计:
当tracking线程创建一个新的关键帧时,就将其设为关键参考帧
与当前关键帧共视点最多的帧设为关键参考帧
位姿估计也同样是在当前帧和参考关键帧进行特征匹配 匹配成功的点数超过一定数目(10)则认为匹配成功。
重定位
遍历所有的候选参考关键帧 yong匹配结果初始化PnPsolver
将两帧不断地进行投影 + BA优化 直到两帧匹配的特征值多于50个 则认为匹配成功 否则跟踪失败
跟踪局部地图
成功估计当前帧的初始化位姿后 基于当前位姿更新局部地图并优化当前帧位姿 流程:
- 更新局部地图
- 将局部地图点投影到当前帧特征点上
- BA 优化当前帧位姿
- 更新地图点观测数值 统计内点个数
- 根据内点个数判断是否跟踪成功
关键帧的创建
关键帧的要求:
当前帧的质量 当前帧的地图点要足够多 同时与参考关键帧的重合程度不能太大
时间间隔
是否进行过重定位 如果进行过重定位 重定位后的位姿不会太准 不能当作参考帧
跟踪线程中地图点的创建与删除
线程初始化会创建地图点
线程中创建新的关键帧会创建新的地图点
参考关键帧
根据参考关键帧估计初始位姿
根据参考帧估计地图点的平均观测距离
参考帧的指定:
创建新关键帧后会将新创建的关键帧设为参考关键帧
将当前关键帧共视程度最高的关键帧设为参考关键帧
局部建图线程
- 插入新的关键帧
- 如果关键帧里有新的地图点 则对新的地图点进行测试
- 创建新地图点(这里是创建了局部地图)
- 进行局部BA优化
- 删除冗余的关键帧
局部建图主函数
run函数:死循环
- 检查缓冲队列中是否含有关键帧
- 处理缓冲队列中的第一个关键帧
- 剔除坏点
- 创建新地图点补充坏点(对极几何或三角化)
- 将当前关键帧与共视关键帧地图点融合
- 局部BA优化
- 剔除冗余关键帧
- 将当前关键帧加入闭环检测中
冗余地图点的标准
- 召回率小于0.25(召回率为实际观测到该地图点的帧数除以理论上该观测到该地图点的帧数)
- 创建的三帧内观测数目少于2(双目为三)
- 地图点在其他地方被剔除了
冗余关键帧的标准
关键帧90%的地图点可以被超过3个其他关键帧观测到
回环检测线程
局部建图线程将关键帧传给回环检测线程 之间同样还是有一个缓冲队列
回环检测
闭环主函数:
检查缓冲队列中是否有未处理的关键帧
检测是否发生回环
单目相机在长时间工作后容易发生极线消失 有较大的飘逸 得到的图像会比之前的图像大很多 这种时候需要进行回环矫正。
闭环检测
闭环检测的标准:
- 连续三帧的场景都在之前出现过
- 这三帧场景具有一定的连续性
若连续四个关键帧都能在数据库中找到对应的闭环匹配关键帧组 且这些关键帧组是连续的 则认为实现闭环
闭环候选关键帧
关键帧的闭环候选关键帧与当前关键帧具有相同的BOW向量但不直接相连的关键帧
回环检测步骤
- 取出缓冲队列头部关键帧 作为当前检测闭环关键帧 设置其不被优化掉
- 如果距离上次回环检测时间太短 则不进行回环检测
- 计算当前关键帧与共视关键帧间的最大相似度
- 根据相似度寻找当前关键帧的闭环候选关键帧
- 在当前的关键帧组和之前的连续关键帧组之间寻找匹配
5.1 构建关键帧组 包括候选关键帧和它的共视关键帧
5.2 遍历之前的连续关键帧组 寻找连续关系 若当前连续关键帧组中某关键帧也在前一帧的候选关键组中 则找到了连续关系
5.3 更新当前关键帧组的连续长度 如果没找到则这个关键帧组的连续长度为0 - 维护循环变量 当前帧的闭环候选关键帧组当作下一帧之前的闭环候选关键帧组
SIM3 相似变换
根据词袋向量进行粗匹配
匹配成功后进行sim3求解
求解成功后优化sim3
优化成功后根将闭环关键帧及其共视帧的所有地图点投影到当前关键帧 进行投影匹配
闭环矫正
步骤:
- sim3位姿传播:将当前关键帧的sim3传播到所有局部关键帧上和局部地图点上
- 地图点融合:将闭环关键帧组的地图点投影到当前关键帧上并进行优化
优化之后将闭环关键帧组的地图点投影到局部关键帧组上 - BA优化:本质图BA优化 优化所有地图点和关键帧位姿 基于本质图 优化的是帧和帧之间的关系
全局BA优化 优化所有地图点和关键帧位姿 基于地图点到关键帧的投影关系 优化的是点和帧之间的关系
共视图、扩展树、本质图
共视图
如果两个关键帧的共视地图点超过了15个 就增加一条边 视为共视。所有的这些边连在一起形成了共视图。
共视图是无向加权图 边的权重就是共视地图点的数目
作用:
跟踪局部地图扩大搜索范围
局部建图里关键帧之间的新建地图点
闭环检测 重定位检测
优化
感性认识共视图的作用:重定位进行候选关键帧的选取时 得到匹配好的候选关键帧 并将和这个关键帧具有共视关系的关键帧全部拿出来 选出评分最高的关键帧作为最终选择出来的候选关键帧
扩展树
对于所有的关键帧 每一个关键帧都能够得到和当前关键帧共视程度最大的关键帧 将当前关键帧和共视程度最大的关键帧连在一起就形成了扩展树
本质图
本质图为扩展树的连接关系
共视关系好的(大与100)的连接关系
形成闭环的连接关系 闭环后的地图点变动后新增加的连接关系
本质图中节点也是所有关键帧
本质图是浓缩版的共视图 只保留权重大与100的边 就是保留了共视程度更高的关键帧
作用:
检测回环时 利用本质图对相似变换sim3进行位姿图优化
零散项
地图点相关
地图信息 = 关键帧数组 + 特征点数组
地图点在关键帧的索引 = 地图点对应的特征点在关键帧的索引
地图点对应特征点 特征点不一定能三角化出地图点
地图点map:关键帧和该地图点在这个关键帧里对应的特征点的索引
参考关键帧:构建这个地图点的关键帧为参考关键帧,如果这个参考关键帧被删除了,则指定第一个观测到这个地图点的关键帧为参考关键帧
双目特征点相关
双目特征点匹配步骤:
粗匹配:根据两个特征点的描述子和金字塔层级进行配对 两个特征点的描述子的距离不能太远 金字塔层级不能相差太多
精匹配:在两个特征点的附近进行窗口滑动 计算两个窗口之间的距离的大小 距离不能过大
亚像素插值:在右目相机的特征点的左右各选取一个特征点 分别计算左目特征点分别到右目特征点以及附近特征点的距离 并拟合成二次函数 求二次函数的最小值 即为最好的匹配结果
删除离群点:匹配距离大于平均匹配距离2.1倍的特征点被认为误匹配