vins中IMU预计分

对于vins-mono，首先需要进行积分处理，对于两帧关键帧之间的图像数据，进行处理：

for (auto &imu_msg : measurement.first)
            {
                double t = imu_msg->header.stamp.toSec();
                double img_t = img_msg->header.stamp.toSec() + estimator.td;
                // std::cout<<"imu_msg: "<<t<<std::endl;
                // std::cout<<"img_msg: "<<img_t<<std::endl;
                std::cout.precision(15);
                // std::cout<<"imu_msg: "<<t<<"  img_msg: "<<img_t<<"  current_time: "<<current_time<<std::endl;
                if (t <= img_t)
                { 
                    // std::cout<<"&&&&&&&&&&&&&&&&&&&&&&&&&* "<<std::endl;
                    std::cout<<"imu_msg: "<<t<<"  img_msg: "<<img_t<<"  current_time: "<<current_time<<std::endl;
                    double dt;
                    if (current_time < 0){
                        current_time = t;
                        dt = 0;
                        // std::cout<<"t - current_time: "<<t - current_time<<std::endl;
                    }
                    else dt = t - current_time;
                    // std::cout<<"dt: "<<dt<<std::endl;
                    ROS_ASSERT(dt >= 0);
                    current_time = t;
                    dx = imu_msg->linear_acceleration.x;
                    dy = imu_msg->linear_acceleration.y;
                    dz = imu_msg->linear_acceleration.z;
                    rx = imu_msg->angular_velocity.x;
                    ry = imu_msg->angular_velocity.y;
                    rz = imu_msg->angular_velocity.z;
                    //cout<<"acc: "<<"dx:"<<dx<<" dy"<<dy<<" dz"<<dz<<endl;
                   // cout<<"gyr: "<<"rx: "<<rx<<" ry: "<<ry<<" rz: "<<rz<<endl;
                    estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
                    //printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);

                }
                else
                {
                    // std::cout<<"********************* "<<std::endl;
                    std::cout<<"imu_msg: "<<t<<"  img_msg: "<<img_t<<"  current_time: "<<current_time<<std::endl;
                    double dt_1 = img_t - current_time;
                    double dt_2 = t - img_t;
                    // std::cout<<"dt_1: "<<dt_1<<"  dt_2: "<<dt_2<<std::endl;
                    current_time = img_t;
                    // std::cout<<"dt_1: "<<dt_1<<std::endl;
                    // std::cout<<"dt_2: "<<dt_2<<std::endl;
                    // std::cout<<"dt_1 + dt_2: "<<dt_1 + dt_2<<std::endl;
                    ROS_ASSERT(dt_1 >= 0);
                    ROS_ASSERT(dt_2 >= 0);
                    ROS_ASSERT(dt_1 + dt_2 > 0);
                    double w1 = dt_2 / (dt_1 + dt_2);
                    double w2 = dt_1 / (dt_1 + dt_2);
                    dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
                    dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
                    dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
                    rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
                    ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
                    rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
                    estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
                    //printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
                }

            }