VINS-Mono中的getMeasurements()函数同步imu数据以及相机特征点帧数据代码逻辑分析（易懂！）

https://www.itread01.com/content/1547395085.html

首先我们来看一下getMeasurements()函数源码：

getMeasurements()
{     //这个函数的作用就是把图像帧和对应的IMU数据们 配对起来,而且IMU数据时间是在图像帧的前面
    std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
 
    while (true)
    {
        //边界判断：数据取完了，说明配对完成
        if (imu_buf.empty() || feature_buf.empty())
            return measurements;
        //边界判断：IMU buf里面所有数据的时间戳都比img buf第一个帧时间戳要早，说明缺乏IMU数据，需要等待IMU数据
        if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            //ROS_WARN("wait for imu, only should happen at the beginning");
            sum_of_wait++;//统计等待的次数
            return measurements;
        }
         //边界判断：IMU第一个数据的时间要大于第一个图像特征数据的时间(说明图像帧有多的)
        if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            ROS_WARN("throw img, only should happen at the beginning");
            feature_buf.pop();
            continue;
        }
        //核心操作：装入视觉帧信息
        sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
        feature_buf.pop();
        //核心操作：转入IMU信息
        std::vector<sensor_msgs::ImuConstPtr> IMUs;
        while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
        {
            IMUs.emplace_back(imu_buf.front());//加入IMUS
            imu_buf.pop();
        }
        IMUs.emplace_back(imu_buf.front());
        if (IMUs.empty())
            ROS_WARN("no imu between two image");
        measurements.emplace_back(IMUs, img_msg);
        //注意：把最后一个IMU帧又放回到imu_buf里去了
        //原因：最后一帧IMU信息是被相邻2个视觉帧共享的
    }
    return measurements;
}
————————————————
版权声明：本文为CSDN博主「Jack Ju」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/qq_40464599/article/details/112392925

在分析getMeasurements()函数源码前，我们先明确一个事情，这部分的代码的目标是什么？然后再去一点一点研究如何一步步实现的。

数据结构: measurements
1、首先，measurements他自己就是一个vector；
2、对于measurements中的每一个measurement，又由2部分组成；
3、第一部分，由sensor_msgs::ImuConstPtr组成的vector；
4、第二部分，一个sensor_msgs::PointCloudConstPtr；
5、这两个sensor_msgs见3.1-6部分介绍。
6、为什么要这样配对(一个PointCloudConstPtr配上若干个ImuConstPtr)？
因为IMU的频率比视觉帧的发布频率要高，所以说在这里，需要把一个视觉帧和之前的一串IMU帧的数据配对起来。

我们直接去分析上述代码中的逻辑，我根据上述花了如下的图。

                                                                                                                                    图1       

 

  图中画的意思是满足第一个条件和第二个条件需要进行裁剪或者等待新的imu信息方之后才能对齐（即不满足对齐条件）。我们一个一个来分析。下述比较的都是时间戳。

首先图中定义了时间轴t的方向，右侧为正方向，同时考虑到队列先进先出的特点，在数轴上给出了front（相对最早的）和back（最近的）。

         对于第一个条件，显然imu的back部分要早于img的front，不能实现二者数据的对齐，右图中显然看出二者没有交集，故，我们需要等待imu新的数据（imu新数据会从back端进入）会慢慢使其imu的back时间戳晚于img的front，从而不满足第一个判断跳进，然后执行其他的程序。

        现在重点解释为何第二个条件不能用于传感器对齐，显然在图中，imu的front要晚于img的front，所以二者必有交集。有交集就意味着可以对齐，但是这里为什么不可以？？原因是，正常情况下，imu的更新频率要快与img图像频率，所以出现图像front早于imu front情况，说明是一开始的时候，即有图像的时候，没有imu，突然来了Imu，这时候，第一帧图像太旧了，就不要这个第一帧的数据，所以，我们后面要将其裁剪，用pop意味着从img的front端裁剪，直到不满足第二个判断条件。

     我又加了个第三个情况，这种情况其实是第一个情况和第二个情况的叠加，双重不满足对齐的条件。

    当上述的2个条件均不满足的时候，我们就可执行程序的后部分，存入可以对齐的img和imu信息。直到把输入的满足传感器imu和img对齐条件的所有队列读空，然后等待新的传感器数据。

                                                                                                                                                                                                                                                                  Jack Ju  于实验室 

                                                                                                                                                                                                                                                                              20210318