ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解3——紧耦合优化模型

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的重投影误差。