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优化;

纯视觉优化

视觉部分的初始化过程和跟踪过程与传统的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 米来进行优化;

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

视觉惯性MAP估计

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

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

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

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

如有疑问,欢迎交流: wx: baobaohaha_ 欢迎对SLAM有兴趣的小伙伴一起交流学习~~

ORBSLAM3的IMU积分求解过程相关推荐

ORB-SLAM3 细读单目初始化过程（终结篇）
本文原创,转载请说明地址:https://blog.csdn.net/shanpenghui/article/details/110522368 一.前言请阅读本文之前最好把ORB-SLAM3的单目 ...
协方差，相关系数及求解过程
一. 协方差 A. 定义协方差用于衡量两个变量的总体误差,方差是协方差的一种特殊情况,即当两个变量是相同的情况D(X)=Cov(X,Y). 期望值分别为E(X),E(Y)的两个实数随机变量X与Y之间 ...
多视图几何总结——基础矩阵、本质矩阵和单应矩阵的求解过程
多视图几何总结--基础矩阵.本质矩阵和单应矩阵的求解过程多视图几何总结--基础矩阵.本质矩阵和单应矩阵的求解过程 1. 说明--其实求解过程大同小异 2. 单应矩阵求解过程 2.1 基于代数误差的线 ...
matlab破损皮革定位,matlab-code-of-TDOAFDOa 干扰源定位代码,应该在的求解过程中有帮助。 276万源代码下载- www.pudn.com...
文件名称: matlab-code-of-TDOAFDOa下载收藏√ [ 5 4 3 2 1 ] 开发工具: matlab 文件大小: 38 KB 上传时间: 2014-05-31 下载 ...
深度学习（四十）优化求解系列（2）简单理解神经网络求解过程-未完待续
对于神经网络的求解过程BP,其实说白了就是复合函数的求导过程,所以我们需要先复习一下高数复合函数的求导过程. 一.复合函数的求导法则 1.复合函数: 此函数是一个包含了三层映射过程的复合函数,为了跟后 ...
两空间异面直线公垂线及交点坐标的求解过程
转自:https://blog.csdn.net/hengfanz/article/details/44223411 已知空间4点,点A.B构成直线L1, 点C.D构成直线L2, 求L2上点P,使P到 ...
数据挖掘机器学习[七]---2021研究生数学建模B题空气质量预报二次建模求解过程：基于Stacking机器学习混合模型的空气质量预测｛含码源+pdf文章｝
相关文章: 特征工程详解及实战项目[参考] 数据挖掘---汽车车交易价格预测[一](测评指标:EDA) 数据挖掘机器学习---汽车交易价格预测详细版本[二]{EDA-数据探索性分析} 数据挖掘机器学习 ...
ansys在求解过程中死机关机的解决办法
ansys是一款功能强大的仿真软件.它用于分析工程领域的结构分析和流体动力学.在使用ansys软件时,必须面对许多复杂的问题,并且计算机会崩溃关机.这导致工作人员的时间浪费以及效率低下. ansys关 ...
布莱克斯科尔斯模型（五) 方程求解过程
布莱克斯科尔斯模型(五) 方程求解过程 posted on 2019-02-12 20:05 luoganttcc 阅读(...) 评论(...) 编辑收藏

ORBSLAM3的IMU积分求解过程

纯视觉优化

纯IMU数据的优化

视觉惯性MAP估计

ORBSLAM3的IMU积分求解过程相关推荐

最新文章

热门文章