OKVIS中imu代码应用在ORB_SLAM2中的解读

Tracking.cpp中调用Eigen::Matrix4d Tcp= propagate(timestamp); // 返回的是pred_Tr_delta：imu预测的当前帧相对上一帧的变换关系T
这个函数又调用了

void Tracking::predictStates(   const Eigen::Matrix4d  &T_sk_to_w,
                                const Eigen::Matrix<double, 9,1>& speed_bias_k,
                                const double * time_pair,
                                std::vector<Eigen::Matrix<double, 7,1> >& measurements,
                                const Eigen::Matrix<double, 6,1> & gwomegaw,
                                const Eigen::Matrix<double, 12, 1>& q_n_aw_babw,
                                Eigen::Matrix4d  * pred_T_skp1_to_w,
                                Eigen::Matrix<double, 3,1>* pred_speed_kp1,
                                Eigen::Matrix<double, 15,15> *covariance,
                                Eigen::Matrix<double, 15,15>  *jacobian)

其中，
T_sk_to_w代入的第一个值是T_s1_to_w，是标定得到的imu到相机坐标的变换关系。KITTI00-02.yaml里面的Rs2c，tsinc计算得来
speed_bias_k是速度+bg+ba共9个数
time_pair是时间间距对
gwomegaw是gravity in local world frame in m/s^2，KITTI00-02.yaml里的gw值
q_n_aw_babw是四种噪声，由KITTI00-02.yaml里na，nw，acc_bias_var，gyro_bias_var四个组成
pred_T_skp1_to_w是输出值，predicted IMU pose at t(k+1) of image indexed k+1
pred_speed_kp1输出值，速度偏差
剩下两个没用到

程序开始：
1，确定初始值，初始位姿的translation是r0=t{TWS} ，初始位姿的旋转部分是CWS0=C{TWS} ，再把位姿转换成四元qWS0=q{TWS} ：

    Eigen::Matrix<double, 3,1>  r_0(T_sk_to_w.topRightCorner<3, 1>());
    Eigen::Matrix<double,3,3> C_WS_0(T_sk_to_w.topLeftCorner<3, 3>());
    Eigen::Quaternion<double>  q_WS_0(C_WS_0);

2，定义积分初值，四元数积分：Δq=(1,0,0,0) ，旋转矩阵积分：∫C=0(3,3) ，旋转矩阵双重积分：∬C=0(3,3) ，加速度积分&#x