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.当前时刻测量噪声传递给下一时刻。 协方差矩阵可以通过递推$Σ_y = AΣ_xA^⊤$计算得到：$Σ_{ik} = {\color{red} A_{k−1}Σ_{ik−1}A^⊤_{k−1}} +{\color{blue} B_{k−1}Σ_nB^⊤_{k−1}}$其中，$Σ_n$ 是测量噪声的协方差矩阵，方差从$i$ 时刻开始进行递推，$Σ_{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的形式为：

其中$(\tilde f_{j-1}-b^a_i)^\wedge$即为Wacc： $w^\wedge=\begin{bmatrix} w_1 \\ w_2 \\w_3 \\ \end{bmatrix}^\wedge = \begin{bmatrix} 0 & -w_3 & w_2 \\ w_3 & 0 & -w_1 \\ -w_2 & w_1 & 0 \\ \end{bmatrix}=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(\phi^\wedge)=I+{sin(\lVert \phi \rVert) \over \lVert \phi \rVert} \phi^\wedge+{1-cos(\lVert \phi \rVert) \over \lVert \phi \rVert ^2} (\phi^\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(\phi^\wedge)=I+\phi^\wedge$ 对应的代码为：

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

根据公式8： $J_r(\phi)=I-{1-cos(\lVert \phi \rVert) \over \lVert \phi \rVert ^2} (\phi^\wedge)+{\lVert \phi \rVert - sin(\lVert \phi \rVert) \over \lVert \phi \rVert ^3} (\phi^\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详细注释的代码持续更新