VINS-Mono代码阅读笔记：addFeatureCheckParallax（）-判断关键帧

addFeatureCheckParallax（）

位于vins_estimator/src/feature_manager.cpp

//添加之前检测到的特征点到feature容器中，计算每一个点跟踪的次数，以及它的视差
//通过检测两帧之间的视差决定次新帧是否作为关键帧
代码来源：https://github.com/ManiiXu/VINS-Mono-Learning，By:ManiiXu
参考原文链接：https://blog.csdn.net/moyu123456789/article/details/102636534

/**
 * @brief   把特征点放入feature的list容器中，计算每一个点跟踪次数和它在次新帧和次次新帧间的视差，返回是否是关键帧
 * @param[in]   frame_count 窗口内帧的个数
 * @param[in]   image 某帧所有特征点的[camera_id,[x,y,z,u,v,vx,vy]]s构成的map,索引为feature_id
 * @param[in]   td IMU和cam同步时间差
 * @return  bool true：次新帧是关键帧;false：非关键帧
 * 参考：https://blog.csdn.net/moyu123456789/article/details/102636534
*/
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;//被跟踪点的个数，非新特征点的个数
    //遍历图像image中所有的特征点，和已经记录了特征点的容器feature中进行比较
    for (auto &id_pts : image)
    {
        //特征点管理器，存储特征点格式：首先按照特征点ID，一个一个存储，每个ID会包含其在不同帧上的位置
        //这里id_pts.second[0].second获取的信息为：xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);

        //迭代器寻找feature list中是否有这feature_id
        //获取feature_id
        int feature_id = id_pts.first;
        //find_if函数，寻找列表中第一个使判别式为true的元素
        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
        {
            return it.feature_id == feature_id;
        });

        //如果没有则新建一个，并添加这图像帧
        if (it == feature.end())
        {
            feature.push_back(FeaturePerId(feature_id, frame_count));
            feature.back().feature_per_frame.push_back(f_per_fra);
        }
        //有的话把图像帧添加进去
        else if (it->feature_id == feature_id)
        {
            /**
             * 如果找到了相同ID特征点，就在其FeaturePerFrame内增加此特征点在此帧的位置以及其他信息，
             * it的feature_per_frame容器中存放的是该feature能够被哪些帧看到，存放的是在这些帧中该特征点的信息
             * 所以，feature_per_frame.size的大小就表示有多少个帧可以看到该特征点
             * */
            it->feature_per_frame.push_back(f_per_fra);
            //last_track_num，表示此帧有多少个和其他帧中相同的特征点能够被追踪到
            last_track_num++;
        }
    }

    //如果窗口内只有一帧或者跟踪到的特征点小于20个，为关键帧
    if (frame_count < 2 || last_track_num < 20)
        return true;

    //计算每个特征在次新帧和次次新帧中的视差
    //遍历每一个feature
    for (auto &it_per_id : feature)
    {
        //计算能被当前帧和其前两帧共同看到的特征点视差
        //it_per_id.feature_per_frame.size()表示该特征点能够被多少帧共视
        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)
        {
            //计算特征点it_per_id在倒数第二帧和倒数第三帧之间的视差，并求所有视差的累加和
            //计算视差参考链接：https://blog.csdn.net/weixin_42846216/article/details/106634318
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            //所有具有视差的特征个数
            parallax_num++;
        }
    }
    //视差等于零，说明没有共同特征
    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);
        //视差总和除以参与计算视差的特征点个数，表示每个特征点的平均视差值
        return parallax_sum / parallax_num >= MIN_PARALLAX;//MIN_PARALLAX=10.0/460.0
    }
}

compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)

计算某个特征点it_per_id在倒数第二帧和倒数第三帧的视差