VINS-Mono代码学习记录（三）--feature_tracker

一、 feature_tracker.h中的变量理解

理解这些变量的含义非常重要，有一些非常容易搞混，让人费解。。。
直接看吧！

  cv::Mat mask;//图像掩膜
    cv::Mat fisheye_mask;//鱼眼相机的掩膜

    // prev_img是上一次发布的帧的图像数据
    // cur_img是光流跟踪的前一帧的图像数据
    // forw_img是光流跟踪的后一帧的图像数据
    cv::Mat prev_img, cur_img, forw_img;
    vector n_pts;//每一帧中新提取的特征点
    vector prev_pts, cur_pts, forw_pts;//对应的图像特征点
    vector prev_un_pts, cur_un_pts;//归一化相机坐标系下的坐标
    vector pts_velocity;//当前帧相对前一帧特征点沿x,y方向的像素移动速度
    vector ids;//能够被跟踪到的特征点的id
    vector track_cnt;//当前帧forw_img中每个特征点被追踪的次数
    map cur_un_pts_map;//把特征点的id和点封装的数据类型
    map prev_un_pts_map;
    camodocal::CameraPtr m_camera;//相机模型
    double cur_time;
    double prev_time;

    static int n_id;//用来作为特征点id，每检测到一个新的特征点，就将n_id作为该特征点的id，然后n_id加1
   

二、readImage(const cv::Mat &_img, double _cur_time)函数理解

有一个非常好用，真的帮助理解的参考，必须要奉上： https://blog.csdn.net/liuzheng1/article/details/89406276
它是我看到的对于帮助理解这个函数最容易的参考资料了。。。

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;

    //【1】判断图像是否需要均衡化
    if (EQUALIZE)//均衡化增强对比度 默认值为1,表示图像太亮或者太暗
    {
        cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//opencv里头的函数
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

   //[2]如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据,将读入的图像赋给当前帧forw_img,同时，还将读入的图像赋给prev_img、cur_img
    if (forw_img.empty())
    {
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        forw_img = img;
    }

    forw_pts.clear();//此时forw_pts还保存的是上一帧图像中的特征点，所以把它清除

    //[3]判断光流跟踪前一帧中特征点规模是否不为0，不为0表示有图像数据点，对其进行光流跟踪
    if (cur_pts.size() > 0)
    {
        TicToc t_o;
        vector status;//status表示 cur_pts和forw_pts中对应点对是否跟踪成功,无法被追踪到的点标记为0
        vector err;
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);//calcOpticalFlowPyrLK() 从cur_pts到forw_pts做LK金字塔光流法

        for (int i = 0; i < int(forw_pts.size()); i++)//光流跟踪结束后，判断跟踪成功的角点是否都在图像内,不在图像内的status置为0
            if (status[i] && !inBorder(forw_pts[i]))
                status[i] = 0;

        //根据status,把跟踪失败的点剔除
        //不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts、prev_pts和cur_pts中剔除
        //prev_pts和cur_pts中的特征点是一一对应的
        //记录特征点id的ids，和记录特征点被跟踪次数的track_cnt也要剔除
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(ids, status);
        reduceVector(cur_un_pts, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
    }

   //[4]光流追踪成功,特征点被成功跟踪的次数track_cnt就加1
    for (auto &n : track_cnt)//track_cnt为每个角点的跟踪次数
        n++;

    //[5]发布这一帧数据
    if (PUB_THIS_FRAME)
    {
        rejectWithF();//通过基本矩阵F剔除outliers

        ROS_DEBUG("set mask begins");
        TicToc t_m;
        setMask();//保证相邻的特征点之间要相隔30个像素,设置mask
        ROS_DEBUG("set mask costs %fms", t_m.toc());

        ROS_DEBUG("detect feature begins");
        TicToc t_t;

        //[6] //计算是否需要提取新的特征点
        int n_max_cnt = MAX_CNT - static_cast(forw_pts.size());
        if (n_max_cnt > 0)
        {
            if(mask.empty())
                cout << "mask is empty " << endl;
            if (mask.type() != CV_8UC1)
                cout << "mask type wrong " << endl;
            if (mask.size() != forw_img.size())
                cout << "wrong size " << endl;

            /**
    * cv::goodFeaturesToTrack()
    * @brief   在mask中不为0的区域检测新的特征点
    * @optional    ref:https://docs.opencv.org/3.1.0/dd/d1a/group__imgproc__feature.html#ga1d6bb77486c8f92d79c8793ad995d541
    * @param[in]    InputArray _image=forw_img 输入图像
    * @param[out]   _corners=n_pts 存放检测到的角点的vector
    * @param[in]    maxCorners=MAX_CNT - forw_pts.size() 返回的角点的数量的最大值
    * @param[in]    qualityLevel=0.01 角点质量水平的最低阈值（范围为0到1，质量最高角点的水平为1），小于该阈值的角点被拒绝
    * @param[in]    minDistance=MIN_DIST 返回角点之间欧式距离的最小值
    * @param[in]    _mask=mask 和输入图像具有相同大小，类型必须为CV_8UC1,用来描述图像中感兴趣的区域，只在感兴趣区域中检测角点
    * @param[in]    blockSize：计算协方差矩阵时的窗口大小
    * @param[in]    useHarrisDetector：指示是否使用Harris角点检测，如不指定，则计算shi-tomasi角点
    * @param[in]    harrisK：Harris角点检测需要的k值
    * @return      void
    */


            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);//MAX_CNT=150
        }
        else
            n_pts.clear();//n_pts是用来存储提取的新的特征点的，在这个条件下表示不需要提取新的特征点，将上一帧中提取的点clear掉
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        //[7]将n_pts中新提取到的角点放到forw_pts中，id初始化-1,track_cnt初始化为1.
        ROS_DEBUG("add feature begins");
        TicToc t_a;
        addPoints();
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }

   //[8]将当前帧数据传递给上一帧
    prev_img = cur_img;//在第一帧处理中还是等于当前帧forw_img
    prev_pts = cur_pts;//在第一帧中不做处理
    prev_un_pts = cur_un_pts;//在第一帧中不做处理
    cur_img = forw_img;//将当前帧赋值给上一帧
    cur_pts = forw_pts;
   
    //[9]//从第二张图像输入后每进行一次循环，最后还需要对匹配的特征点对进行畸变矫正和深度归一化，计算速度
    undistortedPoints();
    prev_time = cur_time;
}

基本上每一个步骤都给了非常详细的解释，自行看代码吧，没什么话说了。。。在readImage()函数中又调用了6个作者自己写的函数，分别是：

bool inBorder(const cv::Point2f &pt)//判断点是否在图像内
void reduceVector(vector &v, vector status)//根据状态status进行重组，将staus中为1的对应点保存下来，为0的对应点对去除掉
void FeatureTracker::setMask()//对跟踪点进行排序并去除密集点
void FeatureTracker::addPoints()//把新提取的特征点放到forw_pts中，id初始化-1,track_cnt初始化为1.
void FeatureTracker::rejectWithF()//通过基本矩阵（F）去除外点outliers
void FeatureTracker::undistortedPoints()//进行畸变矫正

其中，setMask()注释为：

void FeatureTracker::setMask()
{
    //[1]判断是否是鱼眼相机，采取相应的掩膜方式
    if(FISHEYE)
        mask = fisheye_mask.clone();
    else
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
    
    //[2]定义复合数据类型，三个数据类型分别表示特征点被track次数、特征点、特征点的id
    vector>> cnt_pts_id;

   //[3]将当前帧中的特征点，及特征点id和被track的次数打包push进cn_pts_id中
    for (unsigned int i = 0; i < forw_pts.size(); i++)
        cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));

    //[4]按特征点的跟踪次数由大到小排序
    sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair> &a, const pair> &b)
         {
            return a.first > b.first;
         });

    //[5]清空track_cnt，forw_pts，id并重新存入
    forw_pts.clear();
    ids.clear();
    track_cnt.clear();

    //[6]遍历cnt_pts_id，操作目的为使图像提取的特征点更均匀
    for (auto &it : cnt_pts_id)
    {
        if (mask.at(it.second.first) == 255) //当前特征点位置对应的mask值为255，则保留当前特征点，将对应的特征点位置pts，id，被追踪次数cnt分别存入
        {
            forw_pts.push_back(it.second.first);
            ids.push_back(it.second.second);
            track_cnt.push_back(it.first);
            //图片，点，半径，颜色为0表示在角点检测在该点不起作用,粗细（-1）表示填充
            //在mask中将当前特征点周围半径为MIN_DIST的区域设置为0，后面不再选取该区域内的点（使跟踪点不集中在一个区域上）
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }
}

rejectWithF()注释如下：

void FeatureTracker::rejectWithF()
{
    if (forw_pts.size() >= 8)
    {
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;
        //[1]构建cv::Point2f类型的点为cv::findFundamentalMat()做准备
        vector un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            //根据不同的相机模型将二维坐标转换到三维坐标
            //对于PINHOLE（针孔相机）可将像素坐标转换到归一化平面并去畸变
            //对于CATA（卡特鱼眼相机）将像素坐标投影到单位圆内
            Eigen::Vector3d tmp_p;
            m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;//  转换为像素坐标
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());

            m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
        }

        vector status;
        //[2]调用cv::findFundamentalMat对un_cur_pts和un_forw_pts计算F矩阵,RANSAC去除outliers
        cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = cur_pts.size();
        reduceVector(prev_pts, status);//根据status进行重组
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(cur_un_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
        ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
    }
}

undistortedPoints()注释如下：

void FeatureTracker::undistortedPoints()//计算点的在x,y方向上的跟踪速度
{
    //【1】清除cur_un_pts、cur_un_pts_map的状态
    cur_un_pts.clear();
    cur_un_pts_map.clear();
    //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
    
    //[2]给cur_un_pts、cur_un_pts_map装进新的值
    for (unsigned int i = 0; i < cur_pts.size(); i++)
    {
        Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
        Eigen::Vector3d b;
        m_camera->liftProjective(a, b);//转像素坐标
        cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
        cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));
        //printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
    }
    
    // 【3】判断上一帧中特征点点是否为0，不为0则计算点跟踪的速度
    if (!prev_un_pts_map.empty())
    {
        double dt = cur_time - prev_time;
        pts_velocity.clear(); // pts_velocity表示当前帧相对前一帧特征点沿x,y方向的像素移动速度
        for (unsigned int i = 0; i < cur_un_pts.size(); i++)
        {
            if (ids[i] != -1)//如果点不是第一次被track
            {
                std::map::iterator it;
                it = prev_un_pts_map.find(ids[i]);
                if (it != prev_un_pts_map.end())
                {
                    double v_x = (cur_un_pts[i].x - it->second.x) / dt;//计算x方向上的速度
                    double v_y = (cur_un_pts[i].y - it->second.y) / dt;//计算y方向上的速度
                    pts_velocity.push_back(cv::Point2f(v_x, v_y));
                }
                else
                    pts_velocity.push_back(cv::Point2f(0, 0));
            }
            else//点第一次被track
            {
                pts_velocity.push_back(cv::Point2f(0, 0));
            }
        }
    }
    else//为0则push一个值
    {
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            pts_velocity.push_back(cv::Point2f(0, 0));
        }
    }
    
    //[4]当前帧中的点和点的id传递到上一帧
    prev_un_pts_map = cur_un_pts_map;
}

其它几个函数就不注释啦！