ORB-SLAM3论文解读

ORB-SLAM1和2的做者Juan已经毕业了，其工做仍然由同一课题组的Carlos继续完成并撰写论文和代码，此次论文一样对应着开源代码，youtube演示效果惊人，不少场景下无需大量修改便可直接使用，可谓是工做党的KPI，学生党的SCI。
废话少说，直入主题。ORB-SLAM3与前两次做品相比，引入和视觉惯性和多地图模式的SLAM。其算法支持的传感器也有单目、双目、RGBD相机等。在相机的成像模型中，采用了针孔、鱼眼相机模型（见摘要的第一段）。
代码传送门：连接: https://github.com/UZ-SLAMLab/ORB_SLAM3.
论文传送门：连接: https://arxiv.org/pdf/2007.11898.pdf.

git

主要的创新点

大佬的自吹自擂部分中也提到，他们的主要创新点有两点：github

“彻底”基于最大后验估计MAP的VI-SLAM ，不管在初始化阶段仍是运行阶段，都采用了MAP进行状态估计，所以ORB-SLAM3在室内外、大小场景中鲁棒性很好，且精确度是其余方法的2~5倍；如文中所讲，本文的IMU和视觉的组合系统是extemely robust的。
多地图系统，当定位丢失即lost时，ORB-SLAM3会自动创建一个新的小地图，并在revisit的时候进行地图的seamlessly merge，所以，这一算法可以使用不只仅几帧以前的信息，而是运用了全局的信息，可以在bundle adjustmen中利用视差较大的帧来增长BA求解的准确性（由于当视察较小时，求解不许确，且优化容易进入局部极值）。这种方法里对同一特征的观测可能在时间上的间隔较大，由于咱们对其上一次的观测甚至可能出如今以前一张小地图中（中间过程里lost了至少一次）；

数据关联模型

short-term，在短程模式下，路标点一旦退出咱们的视野，咱们将这个路标点丢弃，其在优化时计算效率高，但存在累计偏差，即便从新回到同一地点，也没法实现重定位或者回环检测，位姿图没法构成闭环。
mid-term，在中程模式下，和短程模式相似，累计偏差只有在从新回到地图中已知区域才有可能实现重定位，创建一个封闭的环状位姿图以消除累计偏差。这与大多数SLAM系统中的方法相似。这一方法可以保证局部定位的准确性。
long-term，长期模型中，采用place recognition的方式，例如Dbow词袋库的回环检测与重定位，无视累计偏差和跟踪丢失，直接将当前位姿和以前一记录位姿对齐。并经过图优化的方式消除（平摊）累计偏差。这一方法可以保证大环境重定位的稳定性。在运用词袋库的回环检测模型中，须要被检测帧具备时间一致性，即须要连续三帧均能出发词袋库召回匹配。在此以后，还须要检查是否具备几何一致性，所以，召回的准确性获得了较大提高，然而其召回率却难以获得保证，这种方法的计算过于缓慢，难以将以前创建的小地图充分应用起来。所以ORB-SLAM3先进行几何一致性的检测，随后再对局部一致性（即连续三个存在共视的帧的匹配性）进行检测。其中，时间一致性将连续三个关键帧进行比较，而此处局部一致性将连续三个共视关键帧进行了检查。因为其存在共视，关键点的匹配会更多，因此其准确性和召回率获得了提高，而计算量会少量增大。

系统总体

这部分就直接按照论文中的部分进行依次解读 (fan)(yi)。

算法

Atlas地图集

整个ORB3最牛逼的地方就在这里了，最先在IROS2019的一篇文章，即参考文献[9]中提到。在ORB3中，Atlas地图集由一系列不连续的小地图构成，并可以无缝链接，实现重定位、回环检测、地点识别等功能。当每个新的视觉帧进入流程，跟踪线程当即追踪并定位新一帧的位姿。地图集自己也随着时间逐步优化且会将新的视觉帧通过挑选做为关键帧。在Atlas中，DBoW2被用于重定位、回环检测、地图合并。地图的状态也分为active和non-active两个状态，在跟踪线程的介绍里将会详细介绍这一机制。windows

跟踪线程

跟踪线程被用来根据当前的active地图，实时地跟踪最新一帧的位置，利用最小化重投影偏差的方式实现位姿的最大后验估计MAP。决定新一帧是否做为关键帧加入地图也是在这个线程中完成的。跟踪线程接受IMU、Frame输入，IMU 被预积分处理，而Frame被提取ORB特征，总体跟踪流程中规中矩，没什么值得一提的地方。
当定位跟踪失败时：首先会在全部的Atlas地图集中搜索当前位置的匹配，若是成功，则将当前的地图设置为non-active，而将在Atlas搜索到的地图设置为active，并继续在active的地图上进行局部跟踪。若是在全图搜索失败，则在必定时间后，当前的active地图会强行被设置为non-active，新的小地图将被构建，并设置为active。这样，就的地图的正确性不会被局部定位的失败影响，新旧地图之间的位姿变换关系不肯定性也有但愿被以后的共视减少或者消除。
app

局部建图线程

局部建图线程中规中矩，只有active的地图被添加关键帧，在添加关键帧的时候，重复的关键点被移除。VI-Bundle Adjustment在当前被插入帧先后窗口内被用来提高地图的质量。可是，在Local mapping框图中的后三项，ORB3加入了一些改进，用了他们的novel MAP，后续会介绍。less

回环和地图合并线程

当每个关键帧被插入，地点识别都被启动，这一关键帧被和整个Atlas地图集关键帧进行比较，若是检测到两关键帧是同一地点，分为两种状况：若被召回帧是当前的active地图中的一部分，则须要进行loop correction回环矫正，在回环矫正以后，Full BA在另外一个线程中被悄悄进行，全局位姿图获得优化，总体的地图一致性获得了提高；若被召回帧是属于一个non-active的map，两个地图则会被合并成一个地图并将大地图设置为当前的active地图。这一过程和跟踪并行，不会影响跟踪的实时性。函数

相机模型

相机模型带来的重定位问题

相机模型这块，ORB3将系统中全部与相机模型相关的部分（投影和反投影公式以及其雅可比）提取了出来，让相机模型成为了一个独立的模块儿，以便随时替换其余的相机模型。在重定位中，因为须要使用PnP（Perspective n points）算法，因此须要一个标定好的针孔模型，所以，其余相机模型再也不适用，在此咱们选择MLPnP（Maximum Likelihood PnP）来做为求解算法。这种算法直接应用光线投影做为输入，只须要使用者提供一个将像素反投影为光束的unprojection公式以及相应像素点的坐标就能够求解相机的位姿。oop

相机模型带来的双目问题

在针孔相机双目视觉中，在寻找特征点匹配时，能够直接在另外一个相机的同一行像素上进行搜索，实现快速的匹配，但这一加速技巧在鱼眼相机或者两个不一样的针孔相机上难以实现。所以咱们将一个双目相机看做是两个具备固定位姿变化SE3的单目相机，且两个相机帧之间存在较大的共视区域。两个单目被分别加入BA优化。为了利用双目的优点，若是两个相机之间存在共视区域，这一区域的特征点能够在第一次被看见的时候被三角化，且尺度的不肯定性被双目SE3的尺度信息所消除。在没有共视的区域，特征点依旧按照多视图几何的方式进行三角化。优化

视觉惯性SLAM部分

ORB-SLAM-VI将ORB-SLAM改装成VI-SLAM。但这一算法仍有其局限性，虽然可以实现地图的复用，但其只能用针孔相机且初始化特慢，在此一一叙述。关于预积分和视觉重投影偏差的部分我就再也不多说了。分别将两个残差的信息矩阵加权马氏距离进行求和最小化便是最终的损失函数。在优化时，调整路标点的三维坐标以及机器人状态（R，p，v，q，b）使目标损失函数最小。以下：spa

min ⁡ S ‾ k , X ( ∑ i = 1 k ∥ r I i − 1 , i ∥ Σ I i , i + 1 2 2 + ∑ j = 0 l − 1 ∑ i ∈ K j ρ Hub ( ∥ r i j ∥ Σ i j ) ) \min _{\overline{\mathcal{S}}_{k}, \mathcal{X}}\left(\sum_{i=1}^{k}\left\|\mathbf{r}_{\mathcal{I}_{i-1, i}}\right\|_{\Sigma_{\mathcal{I}_{i, i+1}}^{2}}^{2}+\sum_{j=0}^{l-1} \sum_{i \in \mathcal{K}^{j}} \rho_{\text {Hub }}\left(\left\|\mathbf{r}_{i j}\right\|_{\Sigma_{i j}}\right)\right) Sk,Xmin(i=1∑k∥∥rIi−1,i∥∥ΣIi,i+122+j=0∑l−1i∈Kj∑ρHub (∥rij∥Σij))

IMU初始化

ORB3中的初始化主要在accuracy和efficiency上进行了改进，以得到一个准确的IMU状态初始值。主要有三点改进的insight：1.纯视觉问题能够单独拿出来解决。2.尺度应该做为一个单独的优化变量而不是做为一个隐含变量存在于逆深度、相机位移等包含尺度的量中，这样的优化具备更快的收敛性。3.在初始化的时候应该考虑传感器uncertainty，不然会带来较大的偏差。

纯视觉最大后验估计

在这一过程当中，首先以4Hz的频率插入关键帧，运行2s纯视觉mapping，这时将会有大约10个相机位姿和几百个特征点，对这一系列变量作一个BA，如图2b所示，得到了一条轨迹。

纯惯性最大后验估计

在这一过程当中，咱们肯定须要在IMU信息优化阶段须要估计的变量有：
Y k = { s , R w g , b , v ‾ 0 : k } \mathcal{Y}_{k}=\left\{s, \mathbf{R}_{\mathrm{w} g}, \mathbf{b}, \overline{\mathbf{v}}_{0: k}\right\} Yk={s,Rwg,b,v0:k}
其中s是scale，Rwg是重力旋转方向，b是bias，v顾名思义。在此咱们整一个最大后验估计：
p ( Y k ∣ I 0 : k ) ∝ p ( I 0 : k ∣ Y k ) p ( Y k ) p\left(\mathcal{Y}_{k} \mid \mathcal{I}_{0: k}\right) \propto p\left(\mathcal{I}_{0: k} \mid \mathcal{Y}_{k}\right) p\left(\mathcal{Y}_{k}\right) p(Yk∣I0:k)∝p(I0:k∣Yk)p(Yk)
其中公式就是贝叶斯公式的去掉分母的版本，后验=似然*先验。其中花体的I就是IMU测量值，咱们将公式搞进最大似然argmax有：
Y k ∗ = arg ⁡ max ⁡ Y k ( p ( Y k ) ∏ i = 1 k p ( I i − 1 , i ∣ s , g d i r , b , v ‾ i − 1 , v ‾ i ) ) \mathcal{Y}_{k}^{*}=\underset{\mathcal{Y}_{k}}{\arg \max }\left(p\left(\mathcal{Y}_{k}\right) \prod_{i=1}^{k} p\left(\mathcal{I}_{i-1, i} \mid s, \mathbf{g}_{d i r}, \mathbf{b}, \overline{\mathbf{v}}_{i-1}, \overline{\mathbf{v}}_{i}\right)\right) Yk∗=Ykargmax(p(Yk)i=1∏kp(Ii−1,i∣s,gdir,b,vi−1,vi))
而后假设IMU预积分值分布和参数的先验分布几率密度函数为高斯分布的密度函数，就能够对整个函数整个求对数并加负号，argmax变argmin：
Y k ∗ = arg ⁡ min ⁡ Y k ( ∥ r P ∥ Σ p 2 + ∑ i = 1 k ∥ r I i − 1 , i ∥ Σ I i − 1 , i 2 ) \mathcal{Y}_{k}^{*}=\underset{\mathcal{Y}_{k}}{\arg \min }\left(\left\|\mathbf{r}_{\mathbf{P}}\right\|_{\Sigma_{p}}^{2}+\sum_{i=1}^{k}\left\|\mathbf{r}_{\mathcal{I}_{i-1, i}}\right\|_{\Sigma_{\mathcal{I}_{i-1, i}}}^{2}\right) Yk∗=Ykargmin(∥rP∥Σp2+i=1∑k∥∥rIi−1,i∥∥ΣIi−1,i2)
rp就是先验，后面那个就是针对每一次IMU预积分的求和。这个y函数的最小值就是咱们所须要估计的参数的最大后验估计。在优化上为了保证旋转矩阵还在流形上，咱们整了指数变换来更新这一李群：
R w g n e w = R w g o l d Exp ⁡ ( δ α g , δ β g , 0 ) \mathbf{R}_{\mathrm{wg}}^{\mathrm{new}}=\mathbf{R}_{\mathrm{wg}}^{\mathrm{old}} \operatorname{Exp}\left(\delta \alpha_{\mathrm{g}}, \delta \beta_{\mathrm{g}}, 0\right) Rwgnew=RwgoldExp(δαg,δβg,0)
为了让尺度更新的时候不至于搞出来负数，咱们也用指数更新尺度：
s new = s old exp ⁡ ( δ s ) s^{\text {new }}=s^{\text {old }} \exp (\delta s) snew =sold exp(δs)
这一下纯IMU部分就彻底ok了。值得一提的是，先验选取中，IMU的bias设置为0，使之不会偏离0太远，且初始的协方差矩阵按照IMU的datasheet设定。当完成了IMU的MAP估计后，纯视觉的尺度被对齐，而且重力方向上也与Rwg旋转后的方向对齐，Bias值也获得了一个合理的估计。

视觉惯性联合最大后验估计

这就没啥好说的了，和其余VIO算法同样，作一个VIO的联合BA，如图2a所示。但bias是不变的。且IMU方面的先验是和纯惯性估计的先验是同样的。初始化在2s时，尺度偏差已经收敛到5%之内，此时咱们就称初始化完成了。但VI联合MAP优化是在延时5-15s以后才运行的，为的是保证初始化的效率。VI联合优化后，地图才被称为时mature的，scale的偏差能够收敛到1%之内。

跟踪和建图

跟踪方面和ORB-SLAM2同样，没有什么特别的地方，实际上跟踪问题就是最新两帧的VI-BA优化问题，而t-1帧路标点是fix的。在建图时，须要在滑动窗口和共视图里作一个范围更大的BA，以得到更高的地图准确性，参与建图BA优化的帧因为计算量限制不能把全部帧加进来，而是仅仅包含窗口内的帧and与当前帧有共同观测的帧。有的时候，初始化并不理想，这时候咱们把头100帧或者前75s的keyframe所有归入初始化优化阶段，用于得到一个较好的重力旋转向量和尺度。实际上这个过程就是“初始化很差的时候就让他初始化久一点”，但这一过程当中只优化图2d框框外的变量。当跟踪过程当中小于15个可辨认特征点时，进入visually lost即视觉丢失状态。并采用应对策略：从IMU估计当前位姿，并扩大滑动窗口的长度，将IMU得到的位姿以及当前帧特征点投影到这个大一点的滑动窗口中去寻找匹配。若是这一过程5s后仍没有成功重定位，则开启新的小地图，并将新图切换为active状态。从而进入我们的下一章节：

地图合并和回环检测

做者又说了，这块儿是他最牛逼的地方，牛逼之处主要有两块儿，第一点是咱们前面所说的回环检测与地图合并机制，除此以外，他们用在共视图local window上的一致性检测，代替了时间上的一致性检测，理论上讲，graph上的距离范围内的一致性确实比单纯时间上的一致性要有道理地多，实验效果反正是又好又快:-)

场景识别

首先，咱们用DBoW操做一番获得三个Ka的匹配候选帧Km，而后将Km、Km的n个最佳共视帧、全部这些帧看到的路标点放进local window里，经过词袋库的正向索引获得各个匹配点的2D匹配关系，从而创建匹配点的3D匹配关系。而后须要咱们计算Km到Ka的一个类似变换Sim(3)若尺度不肯定。计算这一变换时，咱们采用Horn algorithm（霍恩法），同时还要作一个RANSAC操做：为获得候选变换，咱们把每一个匹配的点对重投影偏差计算出来，当小于必定阈值，就给这个变换投一票，采用民主共和的方式选出最好的位姿变换T。
获得T后，因为前面是基于采样计算的位姿变换，没有用到所有的特征点匹配信息，咱们须要用上全部信息refine一下这个位姿变换。咱们在local window里找全部与这帧图像中特征点匹配的路标，并使用双向转移偏差做为偏差目标函数来优化这一位姿变换，还用一下Huber来加强一波稳定性。若是inlier的个数ok，咱们再在更小的local window（减少前面的n）中开始第二次的引导匹配。为了验证这一结果，咱们把后续进来的3帧都作一下检验：在Ka的active map中找两个共视帧，而后看这两帧与local windows的匹配点数目是否超过阈值。这一过程持续三帧成功或者连续两帧失败来做为验证成功失败的判断条件。最最最最后用IMU重力再验证一波pitch和roll才算最终经过考验，成为一个place recognition。

纯视觉地图合并

地图合并主要分为缝合和优化两部分

地图缝合

当咱们发现了Ka和Km的匹配发生在不一样小地图之间，就须要对他们进行缝合（wlding）操做。为此咱们将Ka、Km两帧、Ka和Km全部共视帧、全部这些帧的特征点都放进缝合窗口（welding window）。在将地图Ma（最新的地图）里面的特征点放进窗口前，咱们见全部特征点均经过Tma变换到Mm地图（被召回的地图）下。在地图缝合时，须要将两图中重复的特征点进行剔除：对Ma中的每一个特征点都在Mm中寻找其匹配点，若是找到则把Ma中的该点去除，并将此次观测放入Mm中去。同时，共视图也应当进行更新。

地图优化

缝合完以后须要进行优化，调整大的内部关系。优化分为两块：缝合优化和总体优化。首先咱们进行缝合优化：咱们将缝合窗口以外的部分进行固定，再在缝合窗口内部进行一个BA优化，到此咱们的缝合后的地图就能够用于追踪新来的帧了。可是为了提高窗口内外的一致性，利用地图缝合减少总体的累计偏差，咱们须要进行总体的位姿图优化：在优化时，缝合窗口内被固定，窗口外采用essential graph进行优化。此时，回环矫正从缝合窗口传递向整个地图，缝合优化就完成了。

视觉惯性地图合并

和上一节讲的差很少，区别有两点：

若是active的地图是mature的，一切照旧；若是不mature，说明尺度信息暂时不可靠，在缝合时，须要用类似变换Sim(3)变换而不是SE(3)将Ma中的点对齐到Mm。
优化的变量和固定的变量如图所示，Easy。

实验和结论

效果很是好 :-）
结论很是硬 :-）

于2020/7/27 17：30