VINS之IMU预积分的处理流程

参考资料:
[1]http://zhehangt.win/2018/04/24/SLAM/VINS/VINSVIO/

IMU预积分的具体细节和公式推导就不展开说了.我们从源头直接给出IMU预积分的处理流程.

在estimator_node.cpp中
main函数

1.把imu数据和img数据(特征点的集合)存入队列中

ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);

2.对于一个measurement,包含很多个imu_msg和一个img_msg数据,imu数据的时间戳都是小于img_msg的

 for (auto &measurement : measurements)
        {
            //! Step2: 对读取到的IMU进行预积分
            for (auto &imu_msg : measurement.first)
                send_imu(imu_msg);

3.对于每一个IMU数据

void send_imu(const sensor_msgs::ImuConstPtr &imu_msg)
{
    //dt位当前IMU数据和前一个IMU数据的时间差,Vector3d(dx, dy, dz)为当前加速度计数据,Vector3d(rx, ry, rz)为当前角速度计数据
    estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
}

4.pre_integrations[WINDOW_SIZE+1]是针对一个滑动窗口的,其中的每个元素保存的都是两帧之间IMU的预积分数据,

对于每一个IMU数据,要做的事情是:1.更新预积分测量值2.更新预积分协方差矩阵F

 pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);

//! 进入预积分的阶段
propagate(dt, acc, gyr);

//中点积分
//输入:
//_dt:当前IMU数据和前一个IMU数据的时间差
//acc_0:前一个IMU加速度计数据
//gyro_0:前一个IMU陀螺仪数据
// _acc_1:当前IMU加速度计数据
// _gyr_1:当前IMU加速度计数据
//delta_p, delta_q, delta_v:前一个IMU预计分测量值
//linearized_ba, linearized_bg:前一个ba和bg
//输出:
//result_delta_p, result_delta_q, result_delta_v:当前IMU预计分测量值
//result_linearized_ba, result_linearized_bg:当前ba和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);

滑动窗口内所有预积分的信息保存在

IntegrationBase  *pre_integrations[(WINDOW_SIZE + 1)]

class IntegrationBase{
    Eigen::Vector3d delta_p; //平移预积分
    Eigen::Quaterniond delta_q; //旋转预积分
    Eigen::Vector3d delta_v; //速度预积分
    Eigen::Matrix jacobian, covariance; //雅可比矩阵和协方差矩阵
}