ORBSLAM3的IMU积分求解过程

最新推荐文章于 2024-07-23 16:01:39 发布

三轮车的视觉进阶_

最新推荐文章于 2024-07-23 16:01:39 发布

阅读量3.9k

点赞数 9

分类专栏： ORBSLAM3 文章标签： slam 视觉slam ORBSLAM3 IMU 优化 1024程序员节

本文链接：https://blog.csdn.net/qq_25458977/article/details/109189491

版权

ORBSLAM3 专栏收录该内容

3 篇文章 2 订阅

订阅专栏

本博客主要是参考论文:<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优化;

纯视觉优化

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

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

纯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 米来进行优化;

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