ORB_SLAM2视觉惯性紧耦合定位算法详解

ORB_SLAM2的作者在2017年提出了具有地图重用功能的单目视觉惯性定位算法，该算法的具体思路和港科大的VINS有着异曲同工之妙，整体过程可分为下面几个部分：
1.整体流程与基础知识总结
2.基于流型的IMU预积分
3.IMU初始化（视觉惯性联合初始化）
4.紧耦合优化模型
下面主要通过这三个方面对整个思路做详细的说明。还是按照VINS相同的方式来讲解，首先对整个理论知识做详细的分析，然后从代码角度做深入理解。
其中代码参考王京大神的开源方案：
https://github.com/jingpang/LearnVIORB
论文参考主要下面两篇：
（1）Visual-Inertial Monocular SLAM with Map Reuse
（2）On-Manifold Preintegration for Real-Time Visual–Inertial Odometry

1.整体流程与基础知识总结
（1）整体框架
对于整个ORB_SLAM2的框架大家有一定的了解，主要分为三个线程Tracking、LocalMapping和Loopclosing。我对VIO这块的理解目前只局限于前两个线程，因此整体框架就包含前两个线程中的理解。

（2）基础知识总结
这里所提到的基础知识都会在下面的理论推导公式中使用，因此都是十分重要的公式，需要牢记。
①叉积的基本性质
叉积的定义式

叉积的一个最重要的性质，在后面的雅各比矩阵推导中起到至关重要的作用，需要谨记

②指数一阶泰勒近似

这个式子就是以前数学中指数一阶泰勒展开，很简单，因为旋转涉及到李群和李代数的指数转换关系，所以这个式子在后面的优化雅各比推导中很重要。
③李群和李代数指数转换关系


李群和李代数是指数映射关系，是三维向量（旋转向量）与三维旋转矩阵之间的转换，上图中可以看到流型的李群上的切平面（李代数）可以由李群一阶近似（李群扰动表示李代数扰动）

上面的公式参考视觉SLAM十四讲中，原文预积分中给出的是李代数“向量”形式，感觉不太好理解，所以还是用反对称矩阵来表示，其中Jr是李代数上的右雅各比

同样的李代数扰动可以表示李群扰动

指数映射还有另外一个性质，在李群原点处的一阶展开可以推导下面的公式，性质和推导攻入如下

其中注意有这个性质：R*p^*RT = exp((Rp)^)

2.基于流型的IMU预积分
这一部分主要参考论文（2）中的内容，ORB_SLAM2融合惯性数据的算法中预积分处理是参照论文（2）中实现的。前面关于VINS的详解说过预积分计算过程，那篇使用的是四元数旋转求解，和这里的过程是一样的，主要计算三个量，分别是IMU预积分值，预积分测量值的协方差矩阵以及残差的雅各比矩阵，各自用途就不多说，有不懂的可以参照VINS的详解
https://blog.csdn.net/wangshuailpp/article/details/78461171
（1）IMU预积分计算
世界坐标系W、IMU坐标系B以及相机坐标系C的关系和转换如下图2所示，所有的位姿和特征点坐标最终都是在世界坐标系下表示，我们会将相机坐标系下的的状态量首先转换到IMU坐标系下然后再转换到世界坐标系下。TBC代表在IMU坐标系B下相机到IMU之间的外参转换矩阵，TWB=(RWB,wp)代表世界坐标系W下IMU到世界坐标的转换矩阵。关于坐标之间的关系比较重要，刚接触会感觉比较复杂，需要多思考。下面所有的公式和变量前下标代表参考坐标系，例如wp就是世界坐标系下的位移，后下标第一个下标是参考坐标系，例如RWB是世界坐标系下。

由IMU的加速度计和陀螺仪可以获得IMU坐标系下的加速度Ba和角速度BwWB，可以表示成下面的公式（27）（28），公式左边是IMU直接输出的数据。
旋转、平移和速度的导数可表示为公式（29）

世界坐标系下的旋转、平移和速度可由一般的积分公式求得

离散时间下采用欧拉积分可以将上面连续时间积分改写成下面式（30）

联立公式（27）（28）可以得到公式（31）

这时候状态量是世界坐标系下的。设现在有i和j两个相邻关键帧，需要已知i时刻对应的IMU状态量，求解离散时间下j时刻关键帧对应的IMU预积分值，


为避免重复积分采用IMU预积分方法计算i、j时刻的状态量的相对变化。也就是将参考坐标系由世界坐标系转换到IMU的i时刻坐标系下，计算得到IMU状态量在i和j时刻之间的相对量，这个相对量是只取积分算式内的值，而不是旋转、速度和位移的变化量，和VINS的处理方式相同。

从上面的公式中可以看出，整个预积分公式和偏置，噪声都有关系，相当于一个二元方程，因此需要分别分析偏置和噪声对系统的影响，噪声服从高斯正态分布，相对来说比较简单，而偏置服从随机游走模型，关系很复杂，因此两者的处理方式不同。对于噪声分量采用噪声高斯传播方程来求解，而偏置采用一阶泰勒展开方式。总体思路是首先将偏置在i时刻的bi处进行一阶展开，得到公式（44）

这里的数值就是IMU最终的预积分数值，但值得注意的是在初始化使用的IMU预积分是没有偏置的一阶项，也就只是等式右边前面一项而已，等式后面的雅各比矩阵是在初始化完成后进行优化之前才计算而来，得到IMU准确的预计分值（真值），和估计值从而得到IMU的残差。所以我们首先要手推一下上面公式右边的前面一项，注意这里所有的指数映射中都省去了写反对称符号。


至此IMU预积分的数值就计算出来，也就是等式右边的第一项。
（2）预积分雅可比和协方差矩阵
下面需要计算出噪声传播项的协方差矩阵和对应偏置的雅各比矩阵。这部分推导在附录部分A和B，和VINS的过程出奇的一致，只不过这里是将状态量分开讨论的，分别是（deta_Rj,deta_vj,deta_pj）和（deta_Rj-1,deta_vj-1,deta_pj-1）、（nba,nbg）以及（deta_Rj,deta_vj,deta_pj）和（deta_ba,deta_bg）的关系，把两者合并就和VINS相同。，这部分推导相对简单就直接贴公式了。
下面是求协方差矩阵：




系统是服从高斯分布的，协方差矩阵是按照高斯分布计算得到，是个9*9的矩阵。
下面是求预积分对偏置的雅各比矩阵：


（3）残差雅可比



位移残差对应的雅可比如上图所示，J时刻相应的雅可比可按照图片方式进行推导，注意对pi和pj时刻的雅可比公式原文有错误，推导过程已经给出。对于速度残差和旋转的雅可比公式与平移相似，就不进行推导直接给出，至此IMU预计分就完成了。

3.IMU初始化（视觉惯性联合初始化）
终于来到视觉惯性初始化阶段了，这段是视觉和惯性数据融合的第一步，是一段松耦合过程。理论上是将准确的视觉数据（但缺少尺度）和快速的IMU数据（需要重力加速度又存在零偏误差）相结合。具体介绍在VINS的博客中也同样说过，这部分关于最小二乘、尺度收敛等问题请参考我的博客：视觉SLAM常见的QR分解SVD分解等矩阵分解方式求解满秩和亏秩最小二乘问题（最全的方法分析总结）
https://blog.csdn.net/wangshuailpp/article/details/80209863
（1）陀螺仪偏置标定（零偏）
这一部分比较简单，直接联立N-1个相机做旋转矩阵的最小二乘即可，然后通过高斯牛顿方法即可得到零偏bg。需要注意一点，当求出零偏后将其代入预积分公式会重新计算一遍预积分值，使预积分数值更加准确。

（2）尺度恢复和重力加速度预估
首先建立预估状态向量X=[s,gw]，其中s是尺度，gw是世界坐标系下的重力加速度也是第一个相机坐标系下的重力加速度。ORB_SLAM2中世界坐标选取的是第一个相机对应的坐标（VINS则不同），这样做会存在一个问题，因为第一个相机可能自身存在一定的旋转倾斜导致整个世界坐标看起来是歪着的，画轨迹的时候有一种倾斜的即视感，所以我觉得还是尽量固定好z方向，使轨迹没有横滚和俯仰。这里使用了三个关键帧联立视觉和IMU预积分数据构建一个AX=B的最小二乘超定方程，至少需要四个关键帧，采用奇异值分解求最小二乘问题，速度较慢但精度高。


（3）加速度计偏置标定和尺度重力加速度优化
上面计算过程没有考虑到加速度计偏置的影响，使得重力加速度和加速度计偏置难以区分，很有可能会导致系统病态性问题，文中提出了重力加速度的大小G，假设其是一个不变值，优化重力加速度的方向。

最后优化的状态量变成了X=[s,deta_θ,ba],也就是说我们只优化重力加速度的方向，每次优化得到一个deta_θ都会代入到论文中的公式（17）计算出最新的gw然后继续优化，直到变量收敛。

4.紧耦合优化模型
视觉惯性紧耦合优化部分分为三个部分，分别是Tracking、LocalMapping和Loopclosing，我只是学习了前两个部分，所以就简单的介绍下这两个内容
（1）Tracking线程中帧间紧耦合优化
论文中说的也比较清楚，分为两中情况，分别是有地图更新和无地图更新。Tracking线程中一开始是没有地图更新的，地图更新是在LocalMapping和LoopClosing中完成。因此这两种方式是随机切换的。首先讲一下优化方法和边缘化策略

红色虚框是固定的状态量（fix），不会进行优化但是会在优化的时候作为约束项优化其他状态量，浅灰色实框是将要进行边缘化的状态量（marginalize），蓝色实框是视觉重投影误差方程，深灰色实框是无地图更新时候的先验数据（prior），绿色实框是IMU测量误差方程。
（a）是出现地图更新的状态，一开始j是当前帧，i是上一帧，通过视觉重投影误差和IMU测量误差优化当前帧的状态量（pj,vj,bj），上一帧的状态量和Map Points不会进行优化而是作为约束项优化当前状态量。
（b）是（a）优化后的结果（估计值和海塞矩阵H），同时将优化结果
（c）是无地图更新状态，当前帧变成了j+1，此时通过重投影误差和IMU测量误差优化当前帧和上一帧j的状态量（pj,vj,bj, pj+1,vj+1,bj+1），Map Points不会进行优化而是作为约束项优化j和j+1时刻的状态量，同时j时刻的优化结果作为先验数据优化，然后将j时刻的状态量边缘化掉。
（d）边缘化掉j时刻的j+1时刻优化量（估计值和海塞矩阵H）作为下一时刻优化的先验数据。
（e）和（c）的过程一模一样。
（f）和（d）的过程一模一样。
然后一直循环（c）（d）过程，直到LocalMapping和LoopClosing中发生地图更新会回到（a）处重复过程。下面给出两种优化方式的误差方程：
①出现地图更新
首先构建整体优化状态向量，包含当前时刻j的旋转，平移速度，位移，加速度计偏置和陀螺仪偏置。整体误差方程包含视觉重投影误差和IMU测量误差。

视觉重投影方程就是简单的针孔相机重投影方程，并没有什么特别其残差，雅可比计算和信息矩阵与一般视觉无异。

IMU测量误差方程包含两部分，分别是P,V,R和ba,bg偏置。其残差，残差雅可比矩阵和信息矩阵在IMU预积分过程已经结算完成，直接代入即可。

②没出现地图更新
按照前面所说，首先构建整体优化状态向量，包含当前时刻j+1和上一时刻j的旋转，平移速度，位移，加速度计偏置和陀螺仪偏置。整体误差方程包含视觉重投影误差、IMU测量误差和先验误差。

视觉和IMU的误差方程和前面相同，下面看一下先验误差，是j时刻的状态量。

（2）LocalMapping线程
整个BA优化的只优化固定的N帧，而且这N帧是共视程度最高的N个关键帧，在局部窗口中（Local Windows），地图中所有的点都是这些N个关键帧产生，其他的关键帧是在固定窗口中（Fixed Window），只提供与局部地图点的观测值，不进行优化。当然局部地图路标点也需要优化。需要优化的关键帧包含有视觉重投影误差和IMU测量误差。需要优化的特征点包含Local和Fixed Window的重投影误差。