VINS-Mono代码学习记录（五）--estimator_node中的process( )

写在前面

上一节介绍了vins_estimator这个node的main( )函数和几个回调函数，现在接下来继续介绍process( )函数吧。

一、process( )主线程

void process()
{
    while (true)
    {
        //[1]进入循环构建measurements复合数据类型，将imu和图像数据组合起来
        std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
        std::unique_lock<std::mutex> lk(m_buf);//锁定m_buf
        //当 getMeasurements()).size() = 0时wait() 阻塞当前线程，并且在收到imucallback,imagecallback的通知后getMeasurements()).size() != 0即有数据时时才会被解除阻塞
        con.wait(lk, [&]
                 {
            return (measurements = getMeasurements()).size() != 0;
                 });
        // lambda函数的语法定义：（采用了追踪返回类型的方式声明其返回值)
// [capture](parameters) mutable -> return-type{statement;}
// []，捕捉列表，捕捉上下文中的变量以供lambda函数使用
/*[]    表示    空捕捉
 *[=]   表示值  传递方式捕捉所有父作用域的变量(包括this)
 *[&]   表示引用传递方式捕捉所有父作用域的变量(包括this)
 *[var] 表示值  传递方式捕捉变量var
 *[&var]表示引用传递方式捕捉变量var
 *[this]表示值  传递方式捕捉当前this指针 (this。函数体内可以使用Lambda所在类中的成员变量。)
 *其它组合形式：
 * */
        lk.unlock();

        //[2]遍历measurements，其实就是遍历获取每一个img_msg和其对应的imu_msg对数据进行处理
        m_estimator.lock();//上锁
        for (auto &measurement : measurements)
        {
            auto img_msg = measurement.second;
            double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
            //[3]遍历和当前img_msg时间上“对齐”的IMU数据,处理imu数据processIMU()
            for (auto &imu_msg : measurement.first)
            {
                double t = imu_msg->header.stamp.toSec();
                double img_t = img_msg->header.stamp.toSec() + estimator.td;
                //由于一帧image图像消息对应多个IMU消息，所以针对每一个IMU消息的时间戳，
                // 需要判断IMU的时间戳和image消息的时间戳之间的大小关系，也就是需要判断IMU消息早于image消息先收到，
                // 还是IMU消息晚于image消息收到。
                if (t <= img_t)//imu的数据比图像数据早到
                {
                    //第一次的时候current_time值为-1
                    if (current_time < 0)
                        current_time = t;//imu的采样时间
                    double dt = t - current_time;
                    ROS_ASSERT(dt >= 0);
                    current_time = t;
                    //dx,dy,dz分别是IMU在三个轴方向上的线加速度
                    dx = imu_msg->linear_acceleration.x;