VINS-Fusion代码按执行顺序阅读（二）

main()函数的最后一句，std::thread sync_thread{sync_process};
可以看出，只有time0在time1前后0.03s内的两幅图片，才会被发布estimator.inputImage(time, image0, image1);。

// extract images with same timestamp from two topics
void sync_process()
{
    while(1)
    {
        if(STEREO)  // STEREO 取值为1 表示双目，为0 表示单目
        {
            cv::Mat image0, image1;
            std_msgs::Header header;
            double time = 0;
            m_buf.lock();
            if (!img0_buf.empty() && !img1_buf.empty())
            {
                double time0 = img0_buf.front()->header.stamp.toSec();
                double time1 = img1_buf.front()->header.stamp.toSec();
                // 0.003s sync tolerance
                if(time0 < time1 - 0.003)
                {
                    img0_buf.pop();
                    printf("throw img0\n");
                }
                else if(time0 > time1 + 0.003)
                {
                    img1_buf.pop();
                    printf("throw img1\n");
                }
                else
                {
                    time = img0_buf.front()->header.stamp.toSec();
                    header = img0_buf.front()->header;
                    image0 = getImageFromMsg(img0_buf.front());
                    img0_buf.pop();
                    image1 = getImageFromMsg(img1_buf.front());
                    img1_buf.pop();
                    //printf("find img0 and img1\n");
                }
            }
            m_buf.unlock();
            if(!image0.empty())
                estimator.inputImage(time, image0, image1);
        }
        
        std::chrono::milliseconds dura(2);  // 2ms
        std::this_thread::sleep_for(dura);
    }
}

进入estimator类中查看inputImage()类成员函数：
int inputImageCnt;是一个类数据成员
对于双目调用featureFrame = featureTracker.trackImage(t, _img, _img1);，前面阅读的代码派上用场了！这一句话完成了解析上13页（四、前端视觉处理）的部分。

SHOW_TRACK变量的含义？如果为真，则发布消息pubTrackImage(imgTrack, t);
imgTrack为FeatureTracker类的数据成员。

queue > > > > > featureBuf; 为Estimator类的一个数据成员，是一个队列。

进入到Estimator类的一个函数成员void processMeasurements();

void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
{
    inputImageCnt++;
    map>>> featureFrame;
    TicToc featureTrackerTime;

    if(_img1.empty())
        featureFrame = featureTracker.trackImage(t, _img);
    else
        featureFrame = featureTracker.trackImage(t, _img, _img1);
    //printf("featureTracker time: %f\n", featureTrackerTime.toc());

    if (SHOW_TRACK)
    {
        cv::Mat imgTrack = featureTracker.getTrackImage();
        pubTrackImage(imgTrack, t);
    }
    
    if(MULTIPLE_THREAD)  
    {     
        if(inputImageCnt % 2 == 0)
        {
            mBuf.lock();
            featureBuf.push(make_pair(t, featureFrame));
            mBuf.unlock();
        }
    }
    else
    {
        mBuf.lock();
        featureBuf.push(make_pair(t, featureFrame));
        mBuf.unlock();
        TicToc processTime;
        processMeasurements();
        printf("process time: %f\n", processTime.toc());
    }
    
}

进入该函数会一直在此处循环，通过break方式退出的条件是
USE_IMU的含义？推测此处的取值为true

下面进入Estimator类的另一个成员函数，getIMUInterval()，bool Estimator::getIMUInterval(double t0, double t1, vector> &accVector, vector> &gyrVector)
该函数的目的很明确，将时间区间[t0,t1]之间的陀螺仪、加速度计输出的数据从accBuf和gyrBu取出存放在accVector和gyrVector两个vector中。

如果还未初始化，即initFirstPoseFlag为false，则首先进行初始化。
感觉这个地方使用了双矢量定姿（一个是加速度计输出的平均值，一个是重力矢量）。还有些细节的地方没有理解（例如yaw用来干什么？），暂时略过。
最后，初始姿态保存为了Rs[0]，是Estimator的一个类数据成员Matrix3d Rs[(WINDOW_SIZE + 1)];

接下来调用了processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);，完成了之前介绍过的解析4页（二、IMU预积分）的当前时刻及帧间PVQ的求取，还有PVQ增量的误差、协方差及Jacobian。

接下来调用了processImage(feature.second, feature.first);，有一种感觉，这里就是处理的核心了！去功能模块仔细阅读。

void Estimator::processMeasurements()
{
    while (1)
    {
        //printf("process measurments\n");
        pair > > > > feature;
        vector> accVector, gyrVector;
        if(!featureBuf.empty())
        {
            feature = featureBuf.front();
            curTime = feature.first + td;
            while(1)
            {
                if ((!USE_IMU  || IMUAvailable(feature.first + td)))
                    break;
                else
                {
                    printf("wait for imu ... \n");
                    if (! MULTIPLE_THREAD)
                        return;
                    std::chrono::milliseconds dura(5);
                    std::this_thread::sleep_for(dura);
                }
            }
            mBuf.lock();
            if(USE_IMU)
                getIMUInterval(prevTime, curTime, accVector, gyrVector);

            featureBuf.pop();
            mBuf.unlock();

            if(USE_IMU)
            {
                if(!initFirstPoseFlag)
                    initFirstIMUPose(accVector);
                for(size_t i = 0; i < accVector.size(); i++)
                {
                    double dt;
                    if(i == 0)
                        dt = accVector[i].first - prevTime;
                    else if (i == accVector.size() - 1)
                        dt = curTime - accVector[i - 1].first;
                    else
                        dt = accVector[i].first - accVector[i - 1].first;
                    processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
                }
            }
            mProcess.lock();
            processImage(feature.second, feature.first);
            prevTime = curTime;

            printStatistics(*this, 0);

            std_msgs::Header header;
            header.frame_id = "world";
            header.stamp = ros::Time(feature.first);

            pubOdometry(*this, header);
            pubKeyPoses(*this, header);
            pubCameraPose(*this, header);
            pubPointCloud(*this, header);
            pubKeyframe(*this);
            pubTF(*this, header);
            mProcess.unlock();
        }

        if (! MULTIPLE_THREAD)
            break;

        std::chrono::milliseconds dura(2);
        std::this_thread::sleep_for(dura);
    }
}