在Tracking线程中,有几种追踪的方法
跟踪模式 | 应用场景 | 使用频率 | 匹配方法 |
---|---|---|---|
TrackWithMotionModel() | 可以利用上一帧求出速度估计出初始位姿时 | 经常使用 | SearchByProjection() 候选点来自GetFeatureInArea() ,实现前后帧特征点的匹配。BA优化仅优化位姿 |
TrackReferenceKeyFrame() | 没有速度信息/刚完成重定位/恒速模型跟踪失败后 | 较少用到 | SearchByBow() 匹配当前帧和关键帧在同一节点下的特征点,不需要投影,速度快,BA优化仅优化位姿 |
Relocalization() | 跟踪丢失时使用 | 很少使用 | 使用了BoW找到与该帧相似的候选关键帧;遍历候选关键帧用SearchByBoW 进行快速匹配;匹配点足够时用EPnP计算位姿并取出内点做BA优化来优化位姿;如果优化完内点较少则通过关键帧投影生成新的匹配SearchByProjection() ,对匹配结果再做BA优化位姿。 |
TrackLocalMap() | 前面三选一之后再用此进行后处理 | 每一次跟踪都使用 | 先找到局部关键帧,取出这些帧中的所有地图点作为局部地图点;使用SearchByProjection() 将局部地图点和当前帧特征点进行匹配,对匹配结果做BA优化,仅优化位姿 |
在完成了上述这些追踪过程之后,还要做一些善后工作:
- 清除观测不到的地图点;
- 清除恒速模型跟踪中,
UpdateLastFrame()
为当前帧添加的临时地图点(双目和RGBD情况) - 检测并插入关键帧,对于双目和RGBD还会产生新的地图点
- 删除在BA中被检测为外点的地图点
- 如果初始化后不久就跟踪失败(即状态为LOST且前面关键帧个数<=5个),重定位也不起作用,就Reset整个系统
- 记录位姿结果
粗估计:相机位姿初始值
1.根据上一帧和运动模型估计:
运动模型是匀速运动模型,也就是说,当前帧与上一帧之间的相对位姿变化量 = 上一帧和上上帧之间的相对位姿变化量。通过这个可以估计出当前帧的位姿初始值,根据这个值,把上一帧的MApPoints和当前帧的FeaturePoints匹配,之后根据匹配进行优化(?)。如果匹配的点对太少,就要使用关键帧进行估计了。
2.重定位:
如果上一帧跟踪失败了,也就是说根本就没有上一帧的位姿(此时,匀速运动假设也回天无力),所以就要进行重定位。也就是说,要从关键帧数据库中寻找有没有哪个关键帧和当前帧很相似,有的话可能当前帧就在这个关键帧附近,从而可以定位当前帧。
那么怎么去找到这个关键帧呢?步骤如下:
- 首先,根据当前帧的特征点计算出对应的词袋模型,
- 然后把当前帧的词袋模型和数据库中各个关键帧的词袋模型进行匹配,从而筛选出一批候选的关键帧
KeyFrameDatabase::DetectRelocalization()
, - 把当前帧的特征点和候选关键帧的MapPoints匹配。
- 只有在重定位时,才使用了EPnP为了对这些候选关键帧们进一步进行筛选,就要每一个关键帧都用RANSAC迭代计算出一个当前帧的位姿(PnP算法),直到找到一个关键帧和对应位姿,让他和当前帧之间有足够多的inliers。然后再对其进行进一步优化,进行更多的特征点和MapPoints的匹配。如果优化之后的位姿还是很棒(inliers数量多),那么就可以确定当前的位姿了。
精求解当前帧位姿
局部地图跟踪:
Tracking::TrackLocalMap()
已知:前面可以获得的当前帧的位姿初值,当前帧的地图点。 (不那么准确也ok)特征点和MapPoints有了初步的匹配后
思路:利用两级共视关键帧
就会进入这个更精细的求解当前帧位姿的步骤。
步骤如下:
- 根据前面的到的当前帧的地图点来找到能观测到当前帧的一级共视关键帧, 并将这些一级共视关键帧的二级共视关键帧,子关键帧,父关键帧一起作为局部关键帧
- 取出上述局部关键帧中所有的地图点作为局部地图点(这里更新了局部关键帧和局部地图点)
- 在函数
Tracking::SearchLocalPoints()
,将局部地图点投影到当前帧,去掉不再视野内的无效地图点,用剩下的局部地图点和当前帧的特征点进行匹配,匹配见SearchByProjection()
。这里相当于是补充了更多的匹配关系,用之前的地图点和新增的地图点对位姿再次优化。在更新当前帧地图点的被观测程度时,注意:这里被单目观测到一次+1,被双目或者RGBD观测到1次加2。 - 对匹配结果进行BA优化,只优化位姿。
局部地图中有:
- 和当前帧有共视的MapPoints的关键帧
- 一些特殊的MapPoints,这些地图点可以投影到当前的画幅内,他们的平均可视方向与当前帧的方向夹角不大于60度,他们和当前帧光心的距离在一定范围之内(如果距离太远,ORB特征的尺度不变性在有金字塔的情况下也很难保证,匹配出错的概率大)
在计算得到局部地图的同时,还要尽可能进一步将局部地图的地图点和当前帧未匹配的特征点相匹配,最终对这个局部地图进行BA优化
什么时候把一个帧选为关键帧KF
如果当前帧比较重要,则会将其作为 KF 插入 Map 并送入 LocalMapping 线程。ORB-SLAM 的一个特点就是,其插入 KF 的条件很宽松,所以就可以插入很多 KF,而 ORB-SLAM 会在 LocalMapping 线程中再对冗余的KF进行剔除。目的是:不放过可能有用的帧,增强系统的鲁棒性,以应对纯旋转等很难处理的相机运动。
具体的条件是:
- 距离上一次重定位已过去了超过20帧
- LocalMapping 线程正闲置,但如果已经有连续20帧内没有插入过 KF 了,那么 LocalMapping 线程不管忙不忙,都要插入 KF 了
- 当前帧至少追踪了 50 个点(当前帧质量不能太差)
- 当前帧追踪的点中不能有90%以上和其 Reference KF 相同(当前帧不能太冗余)