VINS 细节系列 - getMeasurements（）

 代码逻辑

• imubuf空 或特征点buf空，返回结果
• 满足两个条件，才进行装载数据：

1. 当最新的imu帧（imu_buf.back()） 新过 图像帧（feature_buf.front()+ td ）的时间戳（保证了图像帧之前已经没有imu数据了）
2. 最旧的imu帧（imu_buf.front()） 旧过 图像帧（feature_buf.front()+ td ）的时间戳的（保证了图像帧之前一定要有imu帧）

• 当最新的imu帧（imu_buf.back()） 没有新过 图像帧（feature_buf.front()+ td ）的时间戳的时候, 返回结果。
• 当最旧的imu帧（imu_buf.front()） 没有旧过 图像帧（feature_buf.front()+ td ）的时间戳的时候, 去掉最旧的图像帧。重新进入循环（这种情况只会出现在程序的开头，即，有图像的时候，没有imu，突然来了Imu，这时候，第一帧图像太旧了，就不要这个第一帧的数据）

图解说明比较清晰：

 

 

 

 

 

 

 

 代码 （不限于IMU-camera）

bool
getMeasurements(std::vector<sensor_msgs::ImuConstPtr> &imu_msg, std::vector<ObsPtr> &gnss_msg)
{
    if (imu_buf.empty() || (GNSS_ENABLE && gnss_meas_buf.empty()))
        return false;
    
    double front_g_ts = time2sec(gnss_meas_buf.front()[0]->time);

    // 1、继续等待
    if (!(( imu_buf.back()->header.stamp.toSec() + time_diff_gnss_local) > front_g_ts))
    {
        sum_of_wait++;  
        return false;   
    }  
    
    // 2、
    double front_imu_ts = imu_buf.front()->header.stamp.toSec() + time_diff_gnss_local;
    while (!gnss_meas_buf.empty() && front_imu_ts > front_g_ts)
    {
        ROS_WARN("throw img, only should happen at the beginning");
        gnss_meas_buf.pop();
        front_g_ts = time2sec(gnss_meas_buf.front()[0]->time);
    }   

    gnss_msg = gnss_meas_buf.front();
    gnss_meas_buf.pop();
    

    while ((imu_buf.front()->header.stamp.toSec() + time_diff_gnss_local) < time2sec(gnss_msg[0]->time))
    {
        imu_msg.emplace_back(imu_buf.front());
        imu_buf.pop();
    }
    
    imu_msg.emplace_back(imu_buf.front());

    if (imu_msg.empty())
        ROS_WARN("no imu between two gnss");
    return true;
}