いしょ

[从零写VIO|第七节]——VINS-Mono代码精简版代码详解——IMU预积分(内容|代码)

理论知识之前在另一篇博客也整理过 ——指路

6.1. 在thd_BackEnd线程中`estimator.processIMU()`：
6.2 solveGyroscopeBias()中利用相机旋转约束标定IMU角速度bias后,重新计算所有帧的IMU积分
6.3 visualInitialAlign()视觉惯性联合优化中，全部初始化后，进行滑窗内的预积分矫正
6.4 增加IMU约束边，并计算预积分误差和信息矩阵

这篇博客梳理的是integration_bash.h中的midPointIntegration()中的一系列操作。大框架可见我的另一篇博客estimator.processIMU()部分——指路

1. 为什么要进行IMU积分？

IMU通过加速度计得到线加速度、通过陀螺仪得到角速度。通过对IMU测量值的积分操作，能够获得机器人的位姿信息。

IMU的频率比相机的要高，将第 k 帧和第 k+1 帧之间的所有 IMU 进行积分,可得第 k+1 帧的位置、速度和旋转(PVQ)，作为视觉估计的初始值，示意图如下：

从IMU获取的IMU坐标系下的测量值：

2. 连续时间下的IMU运动模型

对于两个连续的关键帧 $b_k$ 和 $b_{k+1}$ ，其对应的时刻分别是 $t_k$ 、 $t_{k+1}$ ，从第 $t_k$ 时刻的PVQ对IMU的测量值进行积分，可以得到第 $t_{k+1}$ 时刻的PVQ,公式为：

3.为什么要用IMU预积分？

注意，此时的积分中包含IMU坐标系到世界坐标系的瞬时旋转矩阵 $q_{b_t}^w$ ，然而,在优化位姿时,关键帧时刻的IMU坐标系相对于世界坐标系的旋转矩阵会发生变化,那么需要对 IMU 重新进行积分。

因此采用预积分来避免这种重复积分。IMU 预积分将参考坐标系从世界坐标系改为前一帧的IMU坐标系 $b_k$ ,从而积出了两帧之间的相对运动。

代入IMU积分公式得：

IMU的预积分量

整理一下得到IMU的预积分量：

可以看到，预积分量只跟IMU的测量值和IMU的bias有关，我们假设短时间内IMU的bias是保持不变的，重新整理PVQ的公式可得：

IMU的预积分误差

一段时间内IMU构建的预积分量作为测量值，对两个时刻之间的状态量进行约束，得到IMU的预积分误差：

以上的误差中位移，速度，偏置都是直接相减得到的。第二项是关于四元数的旋转误差。
注意，在每次迭代时，关于imu测量得到的预积分量，往往需要利用bias进行矫正，矫正公式如下：

详细解析以及代码的实现可见下方第五节evaluate() 函数的解析。

预积分的离散形式

离散形式有两种方法：中值法和欧拉法。这里只分析中值法：
也就是两个相邻时刻 $k$ 到 $k + 1$ 的位姿是用两个时刻的测量值 $a$ , $w$ 的平均值来计算：

代码：

 void midPointIntegration(double _dt,                             
                          const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,                            
                          const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,                            
                          const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,                            
                          const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,                            
                          Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,                            
                          Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)    
{

4. 误差传播

将测量噪声考虑进来，更改PVQ模型：

预积分误差传播的形式

相关代码在integration_bash.h中的midPointIntegration()：

if(update_jacobian)        
{            
    Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;            
    Vector3d a_0_x = _acc_0 - linearized_ba;            
    Vector3d a_1_x = _acc_1 - linearized_ba;            
    Matrix3d R_w_x, R_a_0_x, R_a_1_x;
            
    R_w_x<<0, -w_x(2), w_x(1),                
           w_x(2), 0, -w_x(0),                
           -w_x(1), w_x(0), 0;            
    R_a_0_x<<0, -a_0_x(2), a_0_x(1),                
             a_0_x(2), 0, -a_0_x(0),                
             -a_0_x(1), a_0_x(0), 0;            
    R_a_1_x<<0, -a_1_x(2), a_1_x(1),                
             a_1_x(2), 0, -a_1_x(0),                
             -a_1_x(1), a_1_x(0), 0;

F矩阵：

MatrixXd F = MatrixXd::Zero(15, 15);            
F.block<3, 3>(0, 0) = Matrix3d::Identity();            
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +                                   
                      -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;            
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;            
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;            
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;            
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;            
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;            
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +                                   -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;            
F.block<3, 3>(6, 6) = Matrix3d::Identity();            
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;            
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;            
F.block<3, 3>(9, 9) = Matrix3d::Identity();            
F.block<3, 3>(12, 12) = Matrix3d::Identity();            //cout<<"A"<

V矩阵

MatrixXd V = MatrixXd::Zero(15,18);            
V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;            
V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;            
V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;            
V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);            
V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;            
V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;            
V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;            
V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;            
V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;            
V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);            
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;            
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;

离散形式的 PVQ 增量误差的 Jacobian 和协方差

预积分误差传播的形式可以简写为：

初始状态：

对应代码：

Q矩阵——噪声项的对角协方差矩阵：

noise = Eigen::Matrix<double, 18, 18>::Zero();      
noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity(); // 加速度的噪声：n_a        
noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity(); // 陀螺仪的噪声：n_g        
noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();        
noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();        
noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity(); // 加速度偏置的噪声n_ba        
noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity(); // 陀螺仪偏置的噪声n_bg

雅可比J和协方差矩阵的迭代：

jacobian = F * jacobian;            
covariance = F * covariance * F.transpose() + V * noise * V.transpose();

至此，midPointIntegration()的代码全部解析完成。

5. IntegrationBase类

    double dt;    
    Eigen::Vector3d acc_0, gyr_0; // b_k    
    Eigen::Vector3d acc_1, gyr_1; // b_k+1
    
    const Eigen::Vector3d linearized_acc, linearized_gyr; // 常量    
    Eigen::Vector3d linearized_ba, linearized_bg; // 用来保存结果
    Eigen::Matrix<double, 15, 15> jacobian, covariance;    
    Eigen::Matrix<double, 15, 15> step_jacobian;    
    Eigen::Matrix<double, 15, 18> step_V;    
    Eigen::Matrix<double, 18, 18> noise;
    
    double sum_dt;    // 用来保存结果
    Eigen::Vector3d delta_p;    
    Eigen::Quaterniond delta_q;    
    Eigen::Vector3d delta_v;
    
    std::vector<double> dt_buf;    
    std::vector<Eigen::Vector3d> acc_buf;    
    std::vector<Eigen::Vector3d> gyr_buf;

IntegrationBase的构造函数：

class IntegrationBase
{  
public:    
    IntegrationBase() = delete;    
    IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,                    
                    const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)        
       : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},          
         linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},            
         jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},          
         sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}
{   
    // IMU噪声参数初始化18*18的噪声矩阵        
    noise = Eigen::Matrix<double, 18, 18>::Zero();        
    noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * 
    Eigen::Matrix3d::Identity(); // 加速度的噪声：n_a        
    noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * 
    Eigen::Matrix3d::Identity(); // 陀螺仪的噪声：n_g        
    noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();        
    noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();        
    noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity(); // 加速度偏置的噪声n_ba        
    noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity(); // 陀螺仪偏置的噪声n_bg    
}

包含的主要函数有：

repropagate()
它的使用次数较少，针对的是从bk到bk+1的PVQ传播矫正和误差传递矫正。

// 对于repropagate()而言，它的使用次数较少，针对的是从bk到bk+1的PVQ传播矫正和误差传递矫正。    
void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)    
{        
    sum_dt = 0.0; // the gap between IMU plot        
    acc_0 = linearized_acc; // a at bk in bk coordinate        
    gyr_0 = linearized_gyr; // w at bk in bk coordinate        
    // 预积分bi to bk        
    delta_p.setZero(); //alpha         
    delta_q.setIdentity(); // gama trans bi to bk        
    delta_v.setZero(); // beta        
    linearized_ba = _linearized_ba; // a bias        
    linearized_bg = _linearized_bg; // w bias        
    jacobian.setIdentity();        
    covariance.setZero();
            
    for (int i = 0; i < static_cast<int>(dt_buf.size()); i++)            
        propagate(dt_buf[i], acc_buf[i], gyr_buf[i]); // 预积分    
}

propagate()
传播函数，使用较多。针对的是bk和bk+1内部的i时刻到i+1时刻的PVQ传播和误差传递。

// 传播函数，针对的是bk和bk+1内部的i时刻到i+1时刻的PVQ传播和误差传递。    
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)    
{        
     dt = _dt;        
     acc_1 = _acc_1; // a at time t=t+dt        
     gyr_1 = _gyr_1; // w at time t=t+dt        
     Vector3d result_delta_p;        
     Quaterniond result_delta_q;        
     Vector3d result_delta_v;        
     Vector3d result_linearized_ba;        
     Vector3d result_linearized_bg;
        
     midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,                            
                        linearized_ba, linearized_bg,                            
                        result_delta_p, result_delta_q, result_delta_v,                            
                        result_linearized_ba, result_linearized_bg, 1);
        
     //checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,        
     //                    linearized_ba, linearized_bg);        
     delta_p = result_delta_p; // alpha_i+1 预积分        
     delta_q = result_delta_q; // gama_i+1 from i+1 coordinate to bk        
     delta_v = result_delta_v; // beta_i+1        
     linearized_ba = result_linearized_ba;        
     linearized_bg = result_linearized_bg;        
     delta_q.normalize();        
     sum_dt += dt; // time for bk to bk+1        
     acc_0 = acc_1; // a_i+1 at bi+1 coordinate        
     gyr_0 = gyr_1; // w_i+1            
 }

void midPointIntegration() 中值积分法
前面已经解析完成。
evaluate() 预积分残差计算
预积分所形成的约束边，对应的代码在void Estimator::problemSolve()中，其中，vertexCams_vec[i]和vertexCams_vec[j]分别为两个时刻的body坐标系的全局位姿估计，对应的是para_Pose。vertexVB_vec[i]和vertexVB_vec[j]分别为两个时刻的速度，陀螺仪和加速度偏置，对应的是para_SpeedBias。

// 添加IMU预积分对应的约束边    
for (int i = 0; i < WINDOW_SIZE; i++)    
{        
    int j = i + 1;        
    if (pre_integrations[j]->sum_dt > 10.0)            
        continue;
    std::shared_ptr<backend::EdgeImu> imuEdge(new backend::EdgeImu(pre_integrations[j]));         
    std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;        
    edge_vertex.push_back(vertexCams_vec[i]);        
    edge_vertex.push_back(vertexVB_vec[i]);        
    edge_vertex.push_back(vertexCams_vec[j]);        
    edge_vertex.push_back(vertexVB_vec[j]);
            
    imuEdge->SetVertex(edge_vertex);        
    problem.AddEdge(imuEdge);    
}

在增加了约束边之后，在接下来的非线性优化中就要对该边的残差进行计算，对应的函数为

void EdgeImu::ComputeResidual() 
{    
    VecX param_0 = verticies_[0]->Parameters();    
    Qd Qi(param_0[6], param_0[3], param_0[4], param_0[5]);    
    Vec3 Pi = param_0.head<3>();
    
    VecX param_1 = verticies_[1]->Parameters();    
    Vec3 Vi = param_1.head<3>();    
    Vec3 Bai = param_1.segment(3, 3);    
    Vec3 Bgi = param_1.tail<3>();
    
    VecX param_2 = verticies_[2]->Parameters();    
    Qd Qj(param_2[6], param_2[3], param_2[4], param_2[5]);    
    Vec3 Pj = param_2.head<3>();
    
    VecX param_3 = verticies_[3]->Parameters();    
    Vec3 Vj = param_3.head<3>();    
    Vec3 Baj = param_3.segment(3, 3);    
    Vec3 Bgj = param_3.tail<3>();
    
    residual_ = pre_integration_->evaluate(Pi, Qi, Vi, Bai, Bgi,                              
                           Pj, Qj, Vj, Baj, Bgj);
                           
    // 信息矩阵求解见第4部分 误差传播
    SetInformation(pre_integration_->covariance.inverse()); // 设置信息矩阵
    // covariance = F * covariance * F.transpose() + V * noise * V.transpose();
}

IMU预积分残差的计算函数evaluate()如下：

Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, 
                                const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,                                          
                                const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)   
{        
    Eigen::Matrix<double, 15, 1> residuals;
    // 雅可比分块        
    // 1. 获取预积分的PQV向量对陀螺仪和加速度偏置的雅克比矩阵        
    Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA); // (0,9) // 位移与加速度bias的雅可比        
    Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG); // (0,12)
    Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG); // (3,12)
    Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);// (6,9)        
    Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG); // (6,12)  
                  
    // 2.计算偏置的变化量        
    Eigen::Vector3d dba = Bai - linearized_ba;        
    Eigen::Vector3d dbg = Bgi - linearized_bg;
                    
    // 3. 根据偏置的更新对预积分的值进行矫正,类似于一阶泰勒        
    Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg); // 矫正后的预积分        
    Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;        
    Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
    
    // 4. 残差的计算        
    // 按照当前估计量对应的预积分值减去根据测量值得到的预积分值的偏差得到        
    residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;        
    residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();        
    residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;        
    residuals.block<3, 1>(O_BA, 0) = Baj - Bai;        
    residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
            
    return residuals;    
}

关于IMU预积分残差计算的知识点:

在求解预积分的时候，是假设IMU的偏置 $b_a$ , $b_w$ 已经确定，实际上在后续的优化中对偏置也进行了优化。那么每次迭代时， $b_a$ , $b_w$ 发生改变，需要重新根据公式求得所有帧之间的IMU的预积分。当偏置变化很小时，可以将预积分值按其对偏置的一阶近似来调整，否则就进行重新传递reprogagte。

利用imu的bias对预积分的矫正公式如下，对应代码中的corrected_delta_q，corrected_delta_v，corrected_delta_p：

IMU预积分残差residuals的计算是按照当前估计量对应的预积分值减去根据测量值得到的预积分值的偏差得到的。

push_back()

    void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)    
    {        
        dt_buf.push_back(dt);        
        acc_buf.push_back(acc);        
        gyr_buf.push_back(gyr);        
        propagate(dt, acc, gyr);    
    }

6.【补充】在VINS-Course出现的IMU预积分

不定期更，代码还没看完…

6.1. 在thd_BackEnd线程中`estimator.processIMU()`：

详情可见我的另一篇博客——指路
利用函数System::getMeasurements()首先获取观测数据，包括图像特征，IMU数据；然后分别进行数据处理。
对于IMU数据：
1. 如果IMU时间戳 $t$ 在相邻两帧的时间戳之间，将当前时间设置为 $t$ ,直接读取角速度和线加速度，然后执行estimator.processIMU()去处理IMU数据。
2. .如果imu的时间戳 $t$ 大于第k+1帧的图像时间戳img_t，利用先前current_time到img_t以及img_t到t的时间差设置权重，然后将加权后的IMU数据用于后面estimator.processIMU()处理。

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{   
   // 第一个IMU数据的处理    
   if (!first_imu) // 初始值为false    
   {        
       first_imu = true;        
       acc_0 = linear_acceleration;        
       gyr_0 = angular_velocity;    
    }    
    // IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];    
    // 是一个数组，里面存放着(WINDOW_SIZE + 1)个指针，指针指向的类型是IntegrationBase    
    // 如果是新的一帧,则新建一个预积分项目    
    if (!pre_integrations[frame_count])    
    {        
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};    
    }    
    // frame_count是窗内图像帧的计数    
    // 一个窗内有多个相机帧，每个相机帧之间又有多个IMU数据    
    if (frame_count != 0)    
    {   
        // push_back()这个成员函数最后调用了另一个成员函数propagate()，且原封不动地把自己的三个输入参数都传递了过去        
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);// k+1 重载，已经进行了预积分        
        //if(solver_flag != NON_LINEAR)        
        tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity); // 输入到图像中的预积分值
        dt_buf[frame_count].push_back(dt);        
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);        
        angular_velocity_buf[frame_count].push_back(angular_velocity);
        // ！！！使用中值法求解当前时刻PVQ，对应公式        
        int j = frame_count;        
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;         
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];        
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();         
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;        
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);        
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;        
        Vs[j] += dt * un_acc;    
     }    
     // 让此时刻的值等于上一时刻的值，为下一次计算做准备    
     acc_0 = linear_acceleration;    
     gyr_0 = angular_velocity;
 }

6.2 solveGyroscopeBias()中利用相机旋转约束标定IMU角速度bias后,重新计算所有帧的IMU积分

   // 4. 给滑窗内的IMU预积分加入角速度bias    
   for (int i = 0; i <= WINDOW_SIZE; i++)        
   Bgs[i] += delta_bg;
   // 5. 重新计算所有帧的IMU积分
   for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)    
    {        
        frame_j = next(frame_i);        
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);    
    }

详情可见我的另一篇博客——指路

6.3 visualInitialAlign()视觉惯性联合优化中，全部初始化后，进行滑窗内的预积分矫正

for (int i = 0; i <= WINDOW_SIZE; i++)    
{        
    pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);    
}

6.4 增加IMU约束边，并计算预积分误差和信息矩阵

详细代码和知识点解析在本博客第5节中的evaluate()解析中。