VINS-Mono关键知识点总结——前端详解

VINS-Mono关键知识点总结——前端详解

VINS-Mono的前端看上去思路是比较简单的，但是如果仔细阅读源码的话还是能看出许多非常有技术性的东西的，下面我就把一些我觉得很有意思的细节码出来，分享一下

1. VINS-Mono的前端流程概述

VINS-Mono的前端整个封装成了一个ROS节点
其订阅的topic是：

1. 相机或者数据集发来的图片

其发布topic是：

1. 由pub_img发布的"feature"，发布的是当前帧的特征点，特征点分装成了sensor_msgs::PointCloudPtr类型，里面包括了每个特征点的（1）归一化平面上的坐标（2）归一化平面上的速度（3）图像平面上的坐标，这三者中归一化平面坐标是在SFM和后端用到，而归一化平面上的速度以及图像平面上的坐标仅仅在进行时间偏差估计时会用到。
2. 由pub_match发布的“feature_img”，发布的是用来调试的带特征点的灰度图，图上特征点的颜色越红代表此特征点被跟踪的帧数越多，越蓝表示越少。

在图片的回调函数中会先对时间进行控制，然后会进入 readImage() 函数 进行光流追踪、特征点提取、特征点提取等步骤

2. setMask() 函数的作用

setMask() 函数主要是和特征点提取有关的，当跟踪的特征点数量没有达到设定的值时就会对图片进行特征点提取，提取采取的算法是OpenCV的Harris角点提取函数

cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);

函数的最后一个参数mask是是设置角点提取的区域，实际上是和输入forw_img尺寸相同的一个Mat数据类型，而这个mask就在这里就是通过setMask()函数生成的，它的作用是使得提取的特征点分布尽可能均匀，setMask() 函数源码如下：

void FeatureTracker::setMask()
{
    if(FISHEYE)
        mask = fisheye_mask.clone();
    else
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
    
    //算法会更加倾向于保留跟踪时间长的特征点
    vector<pair<int, pair<cv::Point2f, int>>> cnt_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])));

    //对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序
    sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
    {
       return a.first > b.first;
    });

    //清空cnt，pts，id并重新存入
    forw_pts.clear();
    ids.clear();
    track_cnt.clear();

    for (auto &it : cnt_pts_id)
    {
        if (mask.at<uchar>(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);

            //在mask中将当前特征点周围半径为MIN_DIST的区域设置为0，后面不再选取该区域内的点（使跟踪点不集中在一个区域上）
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }
}

其大概流程是会根据光流法跟踪下一帧图片forw_img获得的forw_pts的被跟踪的时间进行排序，然后按照被跟踪时间从长到段的顺序以半径MIN_DIST在一张白图上画黑圈，如果画出来的mask大概长下面这个样子（有点密集恐惧症了…）：

将这个mask图输入到goodFeaturesToTrack函数中，函数就只会在白色区域，也就是没有跟踪时长特别长的地区提取特征点，因此会使得我们提取的关键点相对较为均匀，如下图所示：

如果你将setMask函数中排序的部分注释掉，特征点的分布就会变成如下图所示结果：

我个人觉得上面这种做法应该是有利有弊的，对于纹理特征丰富的环境，这种操作使得特征点分布更加均匀是有利于系统鲁棒性的，在ORB SLAM2和SVO等经典框架中，前端都有相应的均匀分布的操作，但是在上面图片中这种纹理特征稀疏的环境下，这种做法就不一定好用了，因为强行使得特征点分布到一些本来就没什么特征的区域，这样可能（因为这里我还没有做实验验证）会使得系统精度下降。

3. rejectWithF() 函数的作用

rejectWithF() 函数的源码如下，其实比较简单：

void FeatureTracker::rejectWithF()
{
    if (forw_pts.size() >= 8)
    {
        ROS_DEBUG("FM ransac begins");//才哟ing的是ransac的方法
        TicToc t_f;

        vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {

            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<uchar> status;
        //调用cv::findFundamentalMat对un_cur_pts和un_forw_pts计算F矩阵
        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);
        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());
    }
}

这里主要是操作是将图像平面上的特征点，投影到归一化平面上然后消除畸变，这一步是通过liftProjective函数实现的，cameral_model功能包通过工厂模式生成了多种相机模型，不同的相机模型的liftProjective函数里面算法不同，但是好像这些相机模型对应的畸变消除算法和《视觉SLAM十四讲》中讲的最基本的那种$k_2,k_4,k_6,p_1,p_2$五个参数的都不同，具体的相机模型我还没有时间去研究，这里我还有两个问题：

1. 为什么FOCAL_LENGTH在程序中设置成了460这个数值？
2. 为什么要将特征带点转化到归一化平面上的像素坐标？

排除这两个问题之外，其他其实也就很简单了，通过findFundamentalMat函数自带的ransac消除外点的功能在光流法之后以及提取新的特征点之前消除光流法更踪错误的点，维持系统精度。

4. addPoints() 函数和 updataID() 函数的作用

addPoints() 函数代码如下：

void FeatureTracker::addPoints()
{
    for (auto &p : n_pts)
    {
        forw_pts.push_back(p);
        ids.push_back(-1);//新提取的特征点id初始化为-1
        track_cnt.push_back(1);//新提取的特征点被跟踪的次数初始化为1
    }
}

updataID() 函数代码如下：

bool FeatureTracker::updateID(unsigned int i)
{
    if (i < ids.size())
    {
        if (ids[i] == -1)
            ids[i] = n_id++;
        return true;
    }
    else
        return false;
}

这两个函数都很短啊，addPoints() 函数主要是将goodFeaturesToTrack() 函数中提取的新的特征点n_pts加入到下一帧的关键点forw_pts中，注意这里将新添加的特征点的id都设为了-1， 而updataID() 函数在这之后将id从n_id开始赋值，而n_id是一个全局变量，从第一个特征点为1开始来一个特征点加一，因此是一个一直增长且不重复的整数，而我们特征点对应的id如果答应出来可以发现是一个乱序且不连续的数列，为什么呢？这可以结合到我们上面讲的setMask() 函数和 rejectWithF() 函数，因为他们分别对这个数列进行了排序和删除～

VINS的前端其实很简单的，其精华主要是在后端的优化的边缘化，前端就总结到这里啦，欢迎指正。

此外，对其他SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结