VINS-Mono工程笔记（四）：后端核心流程

1. 前言

后端主要流程包含在estimator_node.cpp的process线程中，这一章主要对该线程函数内部流程进行梳理。

2.逻辑分析

void process()
{
    while (true)
    {
        std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
        std::unique_lock<std::mutex> lk(m_buf);
        /**
         * 这里执行条件变量con.wait函数并且measurements.size==0，就锁定了当前线程，当前线程会一直被阻塞
         * 直到有另一个线程也使用con变量调用notification函数来唤醒当前线程
         * 当其他线程调用了notification后，这里的wait函数中还需要判断(measurements = getMeasurements()).size() != 0是否为true,
         * 只有当为true的时候才会解除当前线程的阻塞
         * */
        con.wait(lk, [&]
                 {
                    //1.获取在时间上“对齐”的IMU和图像数据的组合
            return (measurements = getMeasurements()).size() != 0;
                 });
        lk.unlock();
        m_estimator.lock();
        //2.遍历measurements，其实就是遍历获取每一个img_msg和其对应的imu_msg对数据进行处理
        for (auto &measurement : measurements)
        {
            auto img_msg = measurement.second;
            double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
            //遍历和当前img_msg时间上“对齐”的IMU数据
            for (auto &imu_msg : measurement.first)
            {
                double t = imu_msg->header.stamp.toSec();
                double img_t = img_msg->header.stamp.toSec() + estimator.td;
                if (t <= img_t)//imu数据比图像数据早到
                {
                    //第一次的时候current_time值为-1
                    if (current_time < 0)
                        current_time = t;
                    //计算时间间隔
                    double dt = t - current_time;
                    ROS_ASSERT(dt >= 0);
                    current_time = t;
                    //dx,dy,dz分别是IMU在三个轴方向上的加速度
                    dx = imu_msg->linear_acceleration.x;
                    dy = imu_msg->linear_acceleration.y;
                    dz = imu_msg->linear_acceleration.z;
                    //rx,ry,rz分别是IMU在三个轴方向上的角速度
                    rx = imu_msg->angular_velocity.x;
                    ry = imu_msg->angular_velocity.y;
                    rz = imu_msg->angular_velocity.z;
                    //对每一个IMU值进行预积分
                    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//图像数据比IMU数据早到
                {
                    double dt_1 = img_t - current_time;
                    //t为imu的时间，dt_2表示imu消息的时间戳和图像数据时间戳之间的差值
                    double dt_2 = t - img_t;
                    current_time = img_t;
                    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 =