VINS_MONO系列：(五)前端特征提取

目录

1、整体流程

2、代码实现

3、探讨与思考

4、参考文献

---

 相关链接

 VINS_MONO系列：(一)总体框架_Derrr...的博客-CSDN博客_vins框架

VINS_MONO系列：(二)IMU预积分详细推导_Derrr...的博客-CSDN博客

VINS_MONO系列：(三)VIO初始化_Derrr...的博客-CSDN博客

VINS_MONO系列：(四)紧耦合VIO实现_Derrr...的博客-CSDN博客

 VINS_MONO系列：(五)前端特征提取_Derrr...的博客-CSDN博客

1、整体流程

VINS_MONO的前端的实现较为简单，核心的内容包括利用cv::goodFeaturesToTrack提取关键点，利用cv::calcOpticalFlowPyrLK对前后帧的关键点进行光流跟踪。

真个前端特征提取的流程如图1所示，回调函数接受到图像数据后会先对图像数据转化为opencv的格式，然后再传入FeatureTracker类中进行处理。处理完以后会更新图像中关键点的id，主要是更新新提取出来的关键。最后将特征提取的结果发布出去，这里发布的数据包括当前帧提取到的关键点的id、速度（图像坐标系下的）、像素坐标和归一化坐标等。

FeatureTracker类主要实现图像关键点的提取、关键点的跟踪以、outlier剔除以及关键点补充等功能。关键点跟踪运用的是金字塔光流跟踪，通过直接调用cv::calcOpticalFlowPyrLK实现。光流跟踪后会利用基础矩阵F对外点进行剔除。剔除外点后，会通过cv::goodFeaturesToTrack来补充新的关键点。最后，计算能实现前后帧追踪关键点的速度。

​

图1 整体流程

2、代码实现

这里主要列举几个比较主要的函数

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

    //如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理
    if (EQUALIZE)
    {
        //自适应直方图均衡
        cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

    if (forw_img.empty())
    {
        //如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据
        //将读入的图像赋给当前帧forw_img，同时还赋给prev_img、cur_img
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        //否则，说明之前就已经有图像读入，只需要更新当前帧forw_img的数据
        forw_img = img;
    }

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

    if (cur_pts.size() > 0)
    {
        TicToc t_o;
        vector status;
        vector<float> err;

        //调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪，得到forw_pts
        //status标记了从前一帧cur_img到forw_img特征点的跟踪状态，无法被追踪到的点标记为0
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

        //将位于图像边界外的点标记为0
        for (int i = 0; i < int(forw_pts.size()); i++)
            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());
    }

    //光流追踪成功,特征点被成功跟踪的次数就加1
    //数值代表被追踪的次数，数值越大，说明被追踪的就越久
    for (auto &n : track_cnt)
        n++;

    //PUB_THIS_FRAME=1 需要发布特征点
    if (PUB_THIS_FRAME)
    {
        //通过基本矩阵剔除outliers
        rejectWithF();

        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;

        //计算是否需要提取新的特征点
        int n_max_cnt = MAX_CNT - static_cast<int>(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(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        ROS_DEBUG("add feature begins");
        TicToc t_a;

        //添将新检测到的特征点n_pts添加到forw_pts中，id初始化-1,track_cnt初始化为1.
        addPoints();

        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }

    //当下一帧图像到来时，当前帧数据就成为了上一帧发布的数据
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;

    //把当前帧的数据forw_img、forw_pts赋给上一帧cur_img、cur_pts
    cur_img = forw_img;
    cur_pts = forw_pts;

    //根据不同的相机模型去畸变矫正和转换到归一化坐标系上，计算速度
    undistortedPoints();
    prev_time = cur_time;
}

3、探讨与思考

4、参考文献

[1] Tong Q ,  Li P ,  Shen S . VINS-Mono[J]. IEEE Transactions on Robotics, 2018.