ORBSLAM3的IMU积分求解过程

本博客主要是参考论文:<Inertial-Only Optimization for Visual-Inertial Initialization> , 也是ORBSLAM3的做者写的关于如何计算IMU预积分,若是想要更好地理解IMU的代码,能够看看这篇论文,很通顺很享受;

(在不少的SLAM问题解决中,都会用到一个词,叫作Manifold, 关于Manifold的解释能够参考这篇博客：Manifold是什么)

基于"单目SLAM的轨迹偏差与IMU所带来的偏差相比是能够忽略的" 这样的假设,本文提出来一种有效的只计算惯性导航数据的MAP估计法, 并用其初始化整个visual-inertial SLAM系统;

1. 本论文主要包括如下几个贡献:

• 下面的关于IMU优化的计算公式,都将IMU噪声的几率模型归入考虑;
• 在优化时,对全部的相关变量进行一次性优化,而不是想以前同样分次求解,致使变量之间的关系解耦;
• 公式相机或机器的初始速度或者高度不进行任何的假设,这也就使得该公式的适用范围更广;
• IMU的误差bias并不为0, 而且加入到后面的优化过程当中;

2. 最大后验初始化(MAXIMUM-A-POSTERIORI)

区别于传统的VI优化 (能够参考2017年的基于ORBSLAM2推出的ORB-VI的优化过程) , 将全部的变量一次性地在bundleAdjustment中进行优化,下面简称 VI-BA, 这篇论文提出一种新的初始化和优化方法;

基于下面几点:

• 基于单目SLAM已是至关的成熟及鲁邦,本文基本假设其只是差一个准确的尺度而已;
• 视觉slam的偏差与IMU数据带来的偏差相比, 是很小的,所以,在进行IMU的inertial-only估计时,将视觉的位姿保持为常量;
• 将scale做为参数加入了优化的过程;
• 将全部的IMU相关量一次性优化,所以采用的也是位置position,线速度 linear velocity, 角速度angulat velocity的全部的变量的交叉协方差(cross-covariance)

具体的初始化过程分为:

1). 纯视觉的MAP估计

2). 纯IMU数据MAP估计

3). 视觉+惯导的MAP优化;

1. 纯视觉优化

视觉部分的初始化过程和跟踪过程与传统的ORBSLAM2一致,惟一的区别在于 关键帧的插入更加频繁(约为4HZ~10HZ) 用于下降IMU偏差带来的不肯定性;  优化过程也是将关键帧以及其所包含的有效地图点送去参与BA优化;

其中利用咱们的标定外参Tbc, 每一帧的KEYFRAME的pose都被转换到IMU的坐标系下, 也就是所谓的body reference 中, 所以每一帧对应在bodyframe中的位姿表示为 其中Ri表示的从body frame到world reference的旋转矩阵, pi表示的是第i帧的一个无尺度的位置;

1. 纯IMU数据的优化

这一步的主要目标是经过上一步中视觉计算出的位姿,得到IMU相关参数的优化结果; 如前面所说的full VI-BA须要一个很好的初值,可是咱们目前并无一个很好的guess, 所以这里不进行full VI-BA, 也就是不将 视觉部分加入的优化的过程. 所以折中的办法是将地图点边缘化(仅仅使用其带来的变量,不参与优化过程,或者构建最小化重投影偏差的方程), 得到轨迹的先验信息,以及先关的信息矩阵, 用这些信息进行IMU数据的优化;

所以, 文章提出了一种方法,也就是将全部轨迹,也就是关键帧的位姿固定,而仅仅优化IMU的相关参数, 打包 以下:

其中, s 也就是整个地图的尺度 ;

  是重力加速度方向, 表示为

表示IMU的加速度和角速度误差;

 是全部的关键帧对应的无尺度的速度;

相对于真值s*v表示的速度, 使用 up-to-scale的速度,使得优化过程更加容易; 同时, 假设在初始化的过程当中bias的值是固定不变的, 同时随机游走random walk几乎没有什么影响; 同时,从一开始就将重力加速度归入到优化公式中; 

所以全部的测量值都是IMU的变量, 同时用表示从第i帧到第j帧的关键帧的预积分;  表示连续的从第0帧到第k帧的IMU积分;  所以在最大后验几率的公式以下:

其中表示在给定第k帧IMU数据的状况下, 从0到k帧的可能的IMU测量值分布可能性; 

考虑到各个变量测量值之间的独立性, 则 又能够表示为:　

若找到知足最大化后验几率的值，　也就是找到值Ｘｋ知足：

下面展现了最大化乘积是怎么转换为最小化和的过程:

由于假设ＩＭＵ的预积分都是服从高斯分布的，　则ＭＡＰ问题转换成公式：

其中和分别表示prior值的残差, 以及IMU数据测量值的残差; 其中能够表示为:

这其中旋转矩阵，速度，以及位移的残差分别表示以下：

具体公式的推到能够参考个人博客ＩＭＵ预积分，　可是须要注意的是在ＯＲＢＳＬＡＭ３的IMU相关公式中,并无随机游走, 且在两个关键帧之间, 误差bias为常数;

接下来就是具体的求解过程当中的一些介绍, 在代码中也能够看到; 由于要求解非线性优化方程, 所以咱们须要设定一个良好的初值; 在代码中,将bais的初值设置为0, 而重力加速度的初值设置为加速度测量值的均值; 在优化scale factor的时候,也对初值进行了假设,分别设置为1, 4, 16 米来进行优化; 

在优化的最后部分, 全部位姿以及地图点都会乘上优化后的尺度因子, 而且根据估计出的重力加速度方向, 再乘上一个旋转矩阵;

视觉惯性MAP估计

这里的估计和以前推出的那版Visual-Inertial中的估计没有什么区别,主要是最小化重投影偏差以及IMU的位姿偏差 .

这一部分的优化中不会对重力加速度和尺度进行直接优化,可是他们已经包含在关键帧的位姿中,做为隐性参数出现;  可是由于上一步求解的是线性系统, 而这一步纠结的是MAP问题, 所以, 所使用的初值会有所不一样;

当全部的优化完成之后, SLAM也就从视觉SLAM(visual SLAM) 切换到了 视觉惯导SLAM(visual-inertial SLAM);

 

具体的介绍就是如此, 后续会追加代码的相关介绍,以及更详细的雅克比矩阵的推导,欢迎交流. 

 

 

 

本文同步分享在 博客“莫奈的三轮车”（CSDN）。
若有侵权，请联系 support@oschina.cn 删除。
本文参与“OSC源创计划”，欢迎正在阅读的你也加入，一块儿分享。