VINS_Fusion代码注释（三）--------------------图像处理部分-----------------------------

VINS_Fusion代码注释（三）--------------------图像处理部分-----------------------------

VINS_Fusion程序注释(一) rosNodeTest.cpp
VINS_Fusion代码注释（二）---------IMU预积分部分---------------
1、具体程序处理顺序

rosNodeTest.cpp(sync_process)->inputimage()->processMeasurements()->分为两部分
a.processIMU()  //上次博客注释
b.processImage() //这次要讲的图像处理部分

2、具体代码

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)
{
    ROS_DEBUG("new image coming ------------------------------------------");
    ROS_DEBUG("Adding feature points %lu", image.size());
//关键帧的判断依据是rotation-compensated过后的parallax足够大，并且tracking上的feature足够多；关键帧会保留在当前Sliding Window中，marginalize掉Sliding Window中最旧的状态，如果是非关键帧则优先marginalize掉。
//VINS里为了控制优化计算量，在实时情况下，只对当前帧之前某一部分帧进行优化，而不是全部历史帧。局部优化帧的数量就是窗口大小。为了维持窗口大小，需要去除旧的帧添加新的帧，也就是边缘化 Marginalization。到底是删去最旧的帧（MARGIN_OLD）还是删去刚刚进来窗口倒数第二帧(MARGIN_SECOND_NEW)，就需要对当前帧与之前帧进行视差比较，如果是当前帧变化很小，就会删去倒数第二帧，如果变化很大，就删去最旧的帧。VINS 里把特征点管理和检查视差放在了同一个函数里，先添加特征点，再进行视差检查。
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    {
        marginalization_flag = MARGIN_OLD;
        //printf("keyframe\n");
    }
    else
    {
        marginalization_flag = MARGIN_SECOND_NEW;
        //printf("non-keyframe\n");
    }

    ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");
    ROS_DEBUG("Solving %d", frame_count);
    ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());
    Headers[frame_count] = header;

    ImageFrame imageframe(image, header);
    imageframe.pre_integration = tmp_pre_integration;
    all_image_frame.insert(make_pair(header, imageframe));
    tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    
/*
当外参完全不知道，VINS可以在线进行估计（rotation）,先在processImage内进行初步估计，在后续优化时，在optimize函数中进行优化。
*/
//外参校准函数
    if(ESTIMATE_EXTRINSIC == 2)
    {
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;
            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
                ROS_WARN("initial extrinsic rotation calib success");
                ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
                //有几个相机，就有几个ric，单目情况下，ric内只有一个值
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                ESTIMATE_EXTRINSIC = 1;//若外参标定成功，则设置flag为1.
            }
        }
    }

    if (solver_flag == INITIAL)
    {
        // monocular + IMU initilization
        if (!S