VINS-Fusion源码逐行解析：与processImage有关的一部分函数

这几天参加个比赛，断更了一阵，今天开始继续补。

上一篇介绍了processImage函数的执行流程，这讲我们来看下与processImage有关，也就是processImage调用的函数，这样有助于我们理解项目的整个过程

首先是addFeatureCheckParalla函数，该函数用于向特征点容器中添加新检测到的特征点并计算当前帧中特征点的视差，确定当前帧是否为关键帧。

我们看下源码：

//该函数用于添加特征点并检查视差变化，确定当前帧是否为关键帧。
//初始化视差统计变量。
//遍历当前帧的特征点，将新特征点添加到 feature 容器中，并更新已有特征点的观测信息。
//检查是否满足特征点视差检查条件，如果满足则返回 true。
//计算满足条件的特征点的视差总和和数量。
//根据视差统计结果返回特征点视差检查结果。
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
    //输出输入的特征点数量。
    ROS_DEBUG("input feature: %d", (int)image.size());
    //输出当前已有的特征点数量。
    ROS_DEBUG("num of feature: %d", getFeatureCount());

    //初始化视差统计变量

    double parallax_sum = 0;//视差总和。
    int parallax_num = 0;//视差数量
    last_track_num = 0;//上一次跟踪的特征点数量。
    last_average_parallax = 0;//上一次的平均视差
    new_feature_num = 0;//新特征点数量
    long_track_num = 0;//长时间跟踪的特征点数量

    // 遍历图像中的特征点
    for (auto &id_pts : image)
    {
        // 创建一个 FeaturePerFrame 对象，存储特征点的当前帧观测信息
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
        // 确保第一个观测是左侧图像的观测
        assert(id_pts.second[0].first == 0);
        // 如果特征点有右侧观测
        if(id_pts.second.size() == 2)
        {
            // 记录右侧图像的观测信息
            f_per_fra.rightObservation(id_pts.second[1].second);
            // 确保第二个观测是右侧图像的观测
            assert(id_pts.second[1].first == 1);
        }

        // 获取特征点的唯一ID
        int feature_id = id_pts.first;
        // 查找该特征点是否已经存在于 feature 容器中
        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
                          {
            return it.feature_id == feature_id;
                          });

        // 如果特征点是新的
        if (it == feature.end())
        {
            // 创建一个新的 FeaturePerId 对象，存储该特征点的信息
            feature.push_back(FeaturePerId(feature_id, frame_count));
            // 添加当前帧的观测信息到新创建的特征点对象中
            feature.back().feature_per_frame.push_back(f_per_fra);
            // 新特征点数量加一
            new_feature_num++;
        }
        // 如果特征点已经存在
        else if (it->feature_id == feature_id)
        {
            // 添加当前帧的观测信息到已有的特征点对象中
            it->feature_per_frame.push_back(f_per_fra);
            // 跟踪的特征点数量加一
            last_track_num++;
            // 如果该特征点的观测帧数大于等于4，则长时间跟踪的特征点数量加一
            if( it-> feature_per_frame.size() >= 4)
                long_track_num++;
        }
    }

    //if (frame_count < 2 || last_track_num < 20)
    //if (frame_count < 2 || last_track_num < 20 || new_feature_num > 0.5 * last_track_num)
    // 如果以下条件之一成立，则返回 true，表示通过特征点视差检查
    if (frame_count < 2 || last_track_num < 20 || long_track_num < 40 || new_feature_num > 0.5 * last_track_num)
        return true;

    // 遍历每一个特征点
    for (auto &it_per_id : feature)
    {
        // 如果特征点在 frame_count-2 和 frame_count-1 之间有观测数据
        if (it_per_id.start_frame <= frame_count - 2 &&
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
        {
            // 计算该特征点的视差并累加到视差总和
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            // 视差计数器加一
            parallax_num++;
        }
    }

    // 如果没有计算出视差，则返回 true
    if (parallax_num == 0)
    {
        return true;
    }
    else
    {
        // 输出视差总和和视差数量的调试信息
        ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
        // 输出当前视差的调试信息
        ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
        // 计算最后的平均视差
        last_average_parallax = parallax_sum / parallax_num * FOCAL_LENGTH;
        // 返回视差是否大于等于最小视差阈值
        return parallax_sum / parallax_num >= MIN_PARALLAX;
    }
}

 该函数首先初始化一些有关计算视差的变量，然后遍历图像中的特征点，通过特征点的ID来检查特征点是否是新的，若是新的则创建新特征点对象，添加了新的特征点

然后对于帧id在 frame_count-2 和 frame_count-1 之间有观测数据的特征点，计算视差总和，最后通过帧数，上一次跟踪的特征点数量、长时间跟踪的特征点数量、新特征点数量和计算的视差来判断当前帧是否通过视差检测，如果返回true，则删掉最旧的一帧，如果返回false，则删掉倒数第二帧，这样可以控制优化计算量，只对当前帧之前的一部分帧进行优化，而不是全部历史帧。

接下来看下getFeatureCount函数，它是统计跟踪次数达到4次的特征点数量的函数

//统计被跟踪次数超过或等于4次的特征点数量
int FeatureManager::getFeatureCount()
{
    //初始化计数器 cnt 为0，统计观测次数大于4的特征点数量
    int cnt = 0;
    //遍历 feature 容器中的每一个特征点
    for (auto &it : feature)
    {
        //对于每个特征点 it，更新其 used_num 属性，该属性等于特征点在 feature_per_frame 中的观测帧数。
        it.used_num = it.feature_per_frame.size();
        //如果观测数量大于4，++
        if (it.used_num >= 4)
        {
            cnt++;
        }
    }
    return cnt;
}

该函数很简单，我们往下看，到了getCorresponding函数，该函数是在两个指定的帧（frame_count_l 和 frame_count_r）之间找到相同特征点的对应关系，并返回这些特征点在两个帧中的三维坐标对，很有意思的是在函数内部如何获得特征点在两个指定帧中索引的做法。

vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{
    // 创建一个存储特征点对应关系的向量
    vector<pair<Vector3d, Vector3d>> corres;
    // 遍历所有特征点
    for (auto &it : feature)
    {
        // 如果特征点在两个指定的帧之间存在观测数据
        if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)
        {
            // 初始化两个三维向量 a 和 b
            Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();
            // 计算特征点在两个指定帧中的索引
            //如果我们需要获取该特征点在帧 frame_count_l 中的观测信息，那么我们需要找到 feature_per_frame 中对应的索引位置
            //frame_count_l - it.start_frame 是该特征点在 feature_per_frame 中的索引
            //假设特征点在第 5 帧被首次观测到（start_frame = 5），我们现在要找到第 7 帧的观测信息（frame_count_l = 7）。
            //feature_per_frame[2] 存储的是特征点在第 7 帧的观测信息
            int idx_l = frame_count_l - it.start_frame;
            int idx_r = frame_count_r - it.start_frame;

            // 获取特征点在左帧中的三维坐标
            a = it.feature_per_frame[idx_l].point;
            // 获取特征点在右帧中的三维坐标
            b = it.feature_per_frame[idx_r].point;
            // 将两个三维坐标对加入到对应关系向量中
            corres.push_back(make_pair(a, b));
        }
    }
    // 返回特征点对应关系向量
    return corres;
}

接下来是重头戏，关于初始化的几个函数，不同模式不同初始化函数