ORB-SLAM3系统详解

目录

Tracking部分

Local Mapping

回环检测和融合

---

可以参考: ORB-SLAM3源码阅读笔记1

论文翻译； 论文解读

一、Tracking部分

Tracking.cc部分与ORB-SLAM3的区别:

TrackWithMotionModel时，默认初值使用IMU预积分的值。

TrackReferenceKeyFrame与orb2一样。

如果上述两个办法失效，仍然没有定位成功，若有IMU，并且当前帧距离上次重定位成功不到1s，则表示丢失LOST；如果超过1s，且当前地图中关键帧数目较多（大于10），则设置为recently-lost。如果recently-lost则用IMU预积分的值。如果连续5秒都没有找回来，再设置为LOST。在纯视觉情况下，如果是recently-lost，则用EPNP重定位。如果是LOST状态（表示真的实在救不回来了），在ATLAS地图集中，若当前地图中关键帧数目小于10，重置当前地图，若超过10，创建新地图加入ATLAS.

TrackLocalMap, 基于local map进行跟踪，如果是IMU模式，并且IMU数据积累很多，则使用视觉+IMU的误差函数进行优化（PoseInertialOptimizationLastFrame）

如果TrackLocalMap失败，但前面成功，若是IMU模式，则设置为recently-lost，否则设置为lost。重定位采用MLPNP算法，考虑到了图像平面特征点像素的不确定度，在正切空间内，优化特征点在正切空间下的投影最小，得到相机的位姿。

如果跟踪成功 或 recently-lost，更新速度，清除无效地图点，按需创建关键帧，如果是lost则，如果是IMU模式并且还未进行IMU初始化，重置当前地图，退出当前跟踪。如果地图中关键帧超过5 并且 纯视觉模式 或 虽然是IMU模式但是已经完成IMU初始化了，创建新的地图。

在Optimizer.cc 里优化的时候，IMU优化的节点包含当前帧位姿+速度，陀螺仪的偏置，加速度计的偏置。第一种边(视觉重投影约束)；第二种边（IMU预积分约束）：两帧之间位姿的变化量与IMU预积分的值偏差尽可能小；第三种边（陀螺仪随机游走约束）：陀螺仪的随机游走值在相近帧间不会相差太多 residual=VG-VGk；第四种边（加速度随机游走约束）：加速度的随机游走值在相近帧间不会相差太多 residual=VA-VAk；【当存在先验时】第五种边（先验约束）：上一帧信息随优化的改变量要尽可能小IMU当前帧和关键帧的误差函数，会用在之后当前帧与上一个非关键帧的优化函数中

优化完统计当前帧跟踪到的地图点数目，判断是否跟踪成功。

IMU预积分：先是利用中值积分计算出来，两个相邻图像帧之间的预积分，然后在使用时，考虑到陀螺仪和加速度计的偏置，再对计算出的预积分进行传递后再使用。

二、Local Mapping

Local Mapping 和TrackLocalMap都是把VI-ORB论文的工作放上来，详情可以看VI-ORB

Local Mapping Run()函数在处理的过程中，设置标志位不接收Tracking线程生成的关键帧，因此Tracking线程关键帧判定时考虑该点，并且在判断是否插入时，如果空闲之间插入，如果不空闲，要根据情况插入。

Local Mapping流程图如下：

​​​​​​​

 

三、回环检测和融合

1. DetectNBestCandidates。在所有关键帧中计算与当前关键帧具有最多相同单词数（max_n），然后相同单词数寻找大于0.8*max_n的关键帧序列lScoreAndMatch。在这个序列中，再计算当前关键帧和lScoreAndMatch里每个相关联的关键帧的相似度分数和，同时找到共视里面（一个小组里）分数最高的关键帧。然后将小组分和分数最高的一个关键帧保存到lAccScoreAndMatch。最后对lAccScoreAndMatch进行根据分数从大到小排序。最后根据当前帧和候选的关键帧是否在同一个map中，将其分到回环帧或是融合候选帧。
2. 然后在候选闭环帧中选出符合要求的一帧作为闭环帧，vpLoopBowCand和vpMergeBowCand分别各选出一帧。在DetectCommonRegionsFromBoW函数里，会对当前帧和闭环候选帧及其共视帧进行特征点匹配、求解和优化sim3，如果成功，说明当前帧和该闭环候选帧是可以建立闭环关系的。然后紧接着会进行融合连续性检测，即将当前帧的3个共视帧与匹配上的闭环帧进行投影匹配，如果匹配数足够，说明当前帧的共视帧与闭环帧也可以建立闭环关系，如果有三个共视帧可以建立闭环关系则返回true，检测到回环！作者论文里也说明了这是他们算法的一个创新点，即检测时间连续性时不用等好几帧，直接拿当前帧的共视帧来做，这样使得检测更快，避免错过回环，即提高了召回率。
3. 如果在上一步中找到了可以和当前帧及当前帧共视帧建立回环关系的闭环帧，但是融合连续性检测没有达到3帧，mnLoopNumCoincidences或者mnMergeNumCoincidences就不会为0(可能为1或2)，那么会执行现在这步。这时下一帧进入DetectCommonRegionsFromBoW( )函数时就会计算回环候选帧-当前帧帧间约束。具体做法为：通过投影寻找闭环帧的局部窗口地图点（遍历每个回环候选帧和其共视最好的5个帧的点云）与当前帧的匹配（searchbyBOW），如果某个回环候选帧的窗口中的某个帧的点云和当前帧点云的sim3匹配(3D-3D RANSAC)收敛了，则窗口更新为该帧的窗口。然后将新窗口中的所有帧的地图点通过投影匹配当前帧，重新进行双边优化，求解出sim3变换，最后进行连续性检测，计算当前帧的公视帧与新的回环帧searchByProjection的匹配内点数目，如果有3个关键帧验证成功则DetectCommonRegionsFromBoW返回true。最终如果进来3个关键帧DetectCommonRegionsFromBoW都正确，则表示融合连续性检测成功
4. 如果融合连续行检测通过，则可以开始回环矫正，在矫正前，如果是IMU模式，根据Sw1w2（两个世界坐标系之间的变换）计算出roll、pitch、yaw。roll，pitch角不应该过大，如果过则步融合。融合函数（CorrectLoop）要求LocalMapping和全局BA线程停止。CorrectLoop的操作有1.将当前帧局部窗口的帧，转到w2坐标系下，将回环帧及其窗口的点与当前帧的地图点进行融合（SearchAndFuse）。把当前帧与其一级和二级关联的关键帧先剔除，当前帧只留下了回环的约束，然后使用4自由度（inertialize）7自由度（visual-only）pose graph优化