ORBSLAM3中融合IMU数据的处理过程，即预积分、协方差传递和预测位姿

以双目IMU为例，即以stereo_inertial_tum_vi为入口，在该函数中通过LoadIMU()加载IMU数据至vImuMeas容器中。

然后开始进入SLAM系统的双目跟踪。

SLAM.TrackStereo(imLeft,imRight,tframe,vImuMeas);

如果是单目VIO模式，把IMU数据存储到mlQueueImuData中，

if (mSensor == System::IMU_STEREO)
        for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
            mpTracker->GrabImuData(vImuMeas[i_imu]);

获得IMU数据之后，获得双目图像GrabImageStereo，开始核心函数Track()。

在ORBSLAM3中认为bias在两帧间不变，然后对IMU数据进行预积分 PreintegrateIMU()。

mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());

因为IMU的频率是大于图像的采集频率，因此在两关键帧之间存在多个IMU数据，将两帧间的imu数据放入mvImuFromLastFrame中，m个imu组数据会有m-1个预积分量，针对预积分位置的不同做不同中值积分的处理IntegrateNewMeasurement()。

---

Step 1.构造协方差矩阵，可参考博客VIO残差函数的构建以及IMU预积分和协方差传递
误差的传递由两部分组成：1.当前时刻的误差传递给下一时刻，2.当前时刻测量噪声传递给下一时刻。
协方差矩阵可以通过递推${\Sigma }_y=A{\Sigma }_x{A}^{\top }$计算得到：$${\Sigma }_{ik}=A_{k-1}{\Sigma }_{ik-1}{A}_{k-1}^{\top }+B_{k-1}{\Sigma }_n{B}_{k-1}^{\top }$$其中，${\Sigma }_n$ 是测量噪声的协方差矩阵，方差从$i$ 时刻开始进行递推，${\Sigma }_{ii}=0$。

cv::Mat A = cv::Mat::eye(9,9,CV_32F);
cv::Mat B = cv::Mat::zeros(9,6,CV_32F);

根据没有更新的旋转矩阵$dR$来更新$dP$与$dV$ ，

dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
dV = dV + dR*acc*dt;

因为本次测量值$acc$为当前坐标系下的加速度，需要将其转换到上一次IMU坐标系下，因此需要乘上未更新的旋转矩阵，计算得到相对于上一帧的位置和速度。

见邱笑晨预积分推导P12，已知矩阵A和B的形式为：
$$A_{j-1}=\left[\begin{array}{ccc}\Delta {\tilde{R}}_{jj-1}& 0& 0\\ -\Delta {\tilde{R}}_{ij-1}({\tilde{f}}_{j-1}-b_i^a{)}^{\wedge }\Delta t& I& 0\\ -\frac{1}{2}\Delta {\tilde{R}}_{ij-1}({\tilde{f}}_{j-1}-b_i^a{)}^{\wedge }\Delta t^2& \Delta tI& I\end{array}\right]$$$$B=\left[\begin{array}{cc}\Delta {\tilde{R}}_{jj-1}\Delta t& 0\\ 0& -\Delta {\tilde{R}}_{ij-1}\Delta t\\ 0& -\frac{1}{2}\Delta {\tilde{R}}_{ij-1}\Delta t^2\end{array}\right]$$
其中$({\tilde{f}}_{j-1}-b_i^a{)}^{\wedge }$即为Wacc：
$$w^{\wedge }={\left[\begin{array}{c}{w}_1\\ w_2\\ w_3\end{array}\right]}^{\wedge }=\left[\begin{array}{ccc}0& -w_3& w_2\\ w_3& 0& -w_1\\ -w_2& w_1& 0\end{array}\right]=W$$
$\Delta {\tilde{R}}_{jj-1}$即旋转量预积分测量值，它由陀螺仪测量值和对陀螺 bias 的估计或猜测计算得到，这里就是dR。

cv::Mat Wacc = (cv::Mat_<float>(3,3) << 0, -acc.at<float>(2), acc.at<float>(1),
                                           acc.at<float>(2), 0, -acc.at<float>(0),
                                           -acc.at<float>(1), acc.at<float>(0), 0);
A.rowRange(3,6).colRange(0,3) = -dR*dt*Wacc;
A.rowRange(6,9).colRange(0,3) = -0.5f*dR*dt*dt*Wacc;
A.rowRange(6,9).colRange(3,6) = cv::Mat::eye(3,3,CV_32F)*dt;
B.rowRange(3,6).colRange(3,6) = dR*dt;
B.rowRange(6,9).colRange(3,6) = 0.5f*dR*dt*dt;

然后求解$\Delta {\tilde{R}}_{jj-1}$和$\Delta {\tilde{R}}_{jj-1}$

Step 2. 构造函数，会根据更新后的bias进行角度积分
IntegratedRotation dRi(angVel,b,dt)根据更新后的bias进行角度积分。
可参考frost的论文对这部分的求解：
$$exp({\varphi }^{\wedge })=I+\frac{sin(\Vert \varphi \Vert )}{\Vert \varphi \Vert }{\varphi }^{\wedge }+\frac{1-cos(\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^2}({\varphi }^{\wedge }{)}^2$$
对应的代码为：

const float d2 = x*x+y*y+z*z;
const float d = sqrt(d2);
cv::Mat W = (cv::Mat_<float>(3,3) << 0, -z, y,
               z, 0, -x,
               -y,  x, 0);
deltaR = I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2;

根据罗德里格斯公式求极限，当$d<eps=1e-4$时，后面的高阶小量忽略掉得到此式。此时指数映射的一阶近似是：
$$exp({\varphi }^{\wedge })=I+{\varphi }^{\wedge }$$
对应的代码为：

if(d<eps)
  {
      deltaR = I + W;
      rightJ = cv::Mat::eye(3,3,CV_32F);
  }

根据公式8：
$$J_r(\varphi )=I-\frac{1-cos(\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^2}({\varphi }^{\wedge })+\frac{\Vert \varphi \Vert -sin(\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^3}({\varphi }^{\wedge }{)}^2$$

rightJ = I - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);

然后强行归一化使dRi符合旋转矩阵的格式，然后继续填充到AB矩阵中。

dR = NormalizeRotation(dR*dRi.deltaR);
A.rowRange(0,3).colRange(0,3) = dRi.deltaR.t();
B.rowRange(0,3).colRange(0,3) = dRi.rightJ*dt;

Step 3.更新协方差
根据公式更新协方差矩阵：

 C.rowRange(0,9).colRange(0,9) = A*C.rowRange(0,9).colRange(0,9)*A.t() + B*Nga*B.t();

这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵

 C.rowRange(9,15).colRange(9,15) = C.rowRange(9,15).colRange(9,15) + NgaWalk;

---

关于IMU预测位姿两个地方会用到：

1. 匀速模型计算速度，但并没有给当前帧位姿赋值；
2. 跟踪丢失时不直接判定丢失，通过这个函数预测当前帧位姿看看能不能拽回来，代替纯视觉中的重定位。

计算当前帧在世界坐标系的位姿,原理都是用预积分的位姿（预积分的值不会变化）与上一帧的位姿（会迭代变化）进行更新 。
参考 ORB-SLAM3详细注释的代码持续更新