VINS-Mono代码解读——视觉跟踪 feature_trackers

前言

本文主要介绍VINS的视觉处理前端的视觉跟踪模块（feature_trackers）。
论文第四章A节（IV. MEASUREMENT PREPROCESSING——A. Vision Processing Front-end）
可以看到其算法主要包括以下内容：
1、对于每一幅新图像，KLT稀疏光流算法对现有特征进行跟踪；
2、检测新的角点特征以保证每个图像特征的最小数目(100-300)；
3、通过设置两个相邻特征之间像素的最小间隔来执行均匀的特征分布；
4、利用基本矩阵模型的RANSAC算法进行外点剔除；
5、对特征点进行去畸变矫正，然后投影到一个单位球面上(对于cata-fisheye camera)。

而文章还提到的视觉处理前端对关键帧的选择将在之后进行讨论。
由于这部分基本没有数学推导所以直接看代码实现吧！

---

流程图

---

对于第5点的一点补充，代码：

m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);

可以根据不同的相机模型将二维坐标转换到三维坐标：
对于CATA（卡特鱼眼相机）将像素坐标投影到单位圆内，这里涉及了鱼眼相机模型
而对于PINHOLE（针孔相机）将像素坐标直接转换到归一化平面（z=1）并采用逆畸变模型(k1,k2,p1,p2)去畸变等。

节点在启动时会先读取相机内参，根据config_file文件中model_type的值决定采用何种相机模型，并创建相应模型的对象指针，读取在该模型下需要的参数。

for (int i = 0; i < NUM_OF_CAM; i++) 
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);//里面保存的是config_file文件
        
void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
    ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}

CameraPtr
CameraFactory::generateCameraFromYamlFile(const std::string& filename)
{
    cv::FileStorage fs(filename, cv::FileStorage::READ);
    if (!fs.isOpened())
    {
        return CameraPtr();
    }
    Camera::ModelType modelType = Camera::MEI;
    if (!fs["model_type"].isNone())
    {
        std::string sModelType;
        fs["model_type"] >> sModelType;
        if (boost::iequals(sModelType, "kannala_brandt"))
        {
            modelType = Camera::KANNALA_BRANDT;
        }
        else if (boost::iequals(sModelType, "mei"))
        {
            modelType = Camera::MEI;
        }
        else if (boost::iequals(sModelType, "scaramuzza"))
        {
            modelType = Camera::SCARAMUZZA;
        }
        else if (boost::iequals(sModelType, "pinhole"))
        {
            modelType = Camera::PINHOLE;
        }
        else
        {
            std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl;
            return CameraPtr();
        }
    }
    switch (modelType)
    {
    case Camera::KANNALA_BRANDT:
    {
        EquidistantCameraPtr camera(new EquidistantCamera);
        EquidistantCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    case Camera::PINHOLE:
    {
        PinholeCameraPtr camera(new PinholeCamera);
        PinholeCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    case Camera::SCARAMUZZA:
    {
        OCAMCameraPtr camera(new OCAMCamera);
        OCAMCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    case Camera::MEI:
    default:
    {
        CataCameraPtr camera(new CataCamera);
        CataCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    }
    return CameraPtr();
}

---

代码实现

输入输出

feature_trackers可被认为是一个单独的模块
输入：图像，即订阅了传感器或者rosbag发布的topic：“/cam0/image_raw”
输出：
1、发布topic：“/feature_trackers/feature_img”
即跟踪的特征点图像，主要是之后给RVIZ用和调试用
2、发布topic：“/feature_trackers/feature”
即跟踪的特征点信息，由/vins_estimator订阅并进行优化
3、发布topic：“/feature_trackers/restart”
即判断特征跟踪模块是否出错，若有问题则进行复位，由/vins_estimator订阅

代码结构

VINS对于视觉跟踪相关的代码在feature_trackers文件夹中。该文件夹内src的文件有：
feature_trackers.cpp/feature_trackers.h：构建feature_trackers类，实现特征点跟踪的函数

函数	功能
bool inBorder	判断跟踪的特征点是否在图像边界内
void reduceVector	去除无法跟踪的特征点
void FeatureTracker::setMask	对跟踪点进行排序并去除密集点
void FeatureTracker::addPoints	添将新检测到的特征点n_pts，ID初始化-1，跟踪次数1
void FeatureTracker::readImage	对图像使用光流法进行特征点跟踪
void FeatureTracker::rejectWithF	通过F矩阵去除outliers
bool FeatureTracker::updateID	更新特征点id
void FeatureTracker::readIntrinsicParameter	读取相机内参
void FeatureTracker::showUndistortion	显示去畸变矫正后的特征点
void FeatureTracker::undistortedPoints	对特征点的图像坐标去畸变矫正，并计算每个角点的速度

feature_trackers类的成员变量

	cv::Mat mask;//图像掩码
    cv::Mat fisheye_mask;//鱼眼相机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;
    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

feature_trackers_node.cpp：main()函数 ROS的程序入口

函数	功能
void img_callback()	ROS的回调函数，对新来的图像进行特征点追踪、处理和发布
int main()	程序入口

parameters.cpp/parameters.h：实现参数的读取和设置
以使用配置文件config\euroc\euroc_config.yaml为例

参数	含义	值
IMAGE_TOPIC	图像的ROS TOPIC	“/cam0/image_raw”
IMU_TOPIC	IMU的ROS TOPIC	“/imu0”
MAX_CNT	特征点最大个数	150
MIN_DIST	特征点之间的最小间隔	30
ROW	图像宽度	752
COL	图像高度	480
FREQ	控制发布次数的频率	10
F_THRESHOLD	ransac算法的门限	1
SHOW_TRACK	是否发布跟踪点的图像	1
EQUALIZE	光太亮或太暗则为1，进行直方图均衡化	1
FISHEYE	如果是鱼眼相机则为1	0
FISHEYE_MASK	鱼眼相机mask图的位置	不需要
CAM_NAMES	相机参数配置文件名	euroc_config
WINDOW_SIZE	滑动窗口的大小	20
STEREO_TRACK	双目跟踪则为1	false
FOCAL_LENGTH	焦距	460
PUB_THIS_FRAME	是否需要发布特征点	false

tic_toc.h：TicToc类，记录时间

可以看到，视觉跟踪的主要实现方法集中在feature_trackers_node.cpp的回调函数和feature_trackers类中，接下来将从main()函数开始对加粗的函数进行具体解读

代码解读(部分内容省略)

---

程序入口 int main(int argc, char **argv)

1、ros初始化和设置句柄，设置logger级别

ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

2、读取如config->euroc->euroc_config.yaml中的一些配置参数

readParameters(n);

3、读取每个相机实例读取对应的相机内参，NUM_OF_CAM=1为单目

 for (int i = 0; i < NUM_OF_CAM; i++) 
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);

4、判断是否加入鱼眼mask来去除边缘噪声

5、订阅话题IMAGE_TOPIC(如/cam0/image_raw)，执行回调函数img_callback

ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

6、发布feature，实例feature_points，跟踪的特征点，给后端优化用
发布feature_img，实例ptr，跟踪的特征点图，给RVIZ用和调试用

pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);

---

回调函数 void img_callback(const sensor_msgs::ImageConstPtr &img_msg)

该函数是ROS的回调函数，主要功能包括：readImage()函数对新来的图像使用光流法进行特征点跟踪，并将追踪的特征点封装成feature_points发布到pub_img的话题下，将图像封装成ptr发布在pub_match下。

1、判断是否是第一帧

if(first_image_flag)

2、判断时间间隔是否正确，有问题则restart

if(img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time) 

3、发布频率控制，并不是每读入一帧图像，就要发布特征点，通过判断间隔时间内的发布次数
（不发布的时候也是会执行readImage() 读取图像进行处理）

if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
	PUB_THIS_FRAME = true;
	// 时间间隔内的发布频率十分接近设定频率时，更新时间间隔起始时刻，并将数据发布次数置0
   if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
	{
	first_image_time = img_msg->header.stamp.toSec();
	pub_count = 0;
	}
}
else
	PUB_THIS_FRAME = false;

4、将图像编码8UC1转换为mono8

5、单目时：FeatureTracker::readImage() 函数读取图像数据进行处理

trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());

6、更新全局ID

7、如果PUB_THIS_FRAME=1则进行发布
将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img;
将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match

---

图像特征跟踪的主要处理函数 void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)

1、createCLAHE() 判断并对图像进行自适应直方图均衡化；
2、calcOpticalFlowPyrLK() 从cur_pts到forw_pts做LK金字塔光流法；
3、根据status，把跟踪失败的和位于图像边界外的点剔除，剔除时不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts、prev_pts、cur_pts，记录特征点id的ids，和记录特征点被跟踪次数的track_cnt中剔除；
4、setMask() 对跟踪点进行排序并依次选点，设置mask：去掉密集点，使特征点分布均匀
5、rejectWithF() 通过基本矩阵F剔除outliers
6、goodFeaturesToTrack() 寻找新的特征点(shi-tomasi角点)，添加(MAX_CNT - forw_pts.size())个点以确保每帧都有足够的特征点
7、addPoints()向forw_pts添加新的追踪点，id初始化-1，track_cnt初始化为1
8、undistortedPoints() 对特征点的图像坐标根据不同的相机模型进行去畸变矫正和深度归一化，计算每个角点的速度

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

    //如果EQUALIZE=1，表示太亮或则太暗
    if (EQUALIZE)//判断是否进行直方图均衡化处理
    {
        //自适应直方图均衡
        //createCLAHE(double clipLimit, Size tileGridSize)
        cv::Ptr<cv::CLAHE> 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.clear();//此时forw_pts还保存的是上一帧图像中的特征点，所以把它清除

    if (cur_pts.size() > 0)
    {
        TicToc t_o;
        vector<uchar> 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()
            * @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);
        }
        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;
}

---

void FeatureTracker::setMask()

该函数主要用于对跟踪点进行排序并去除密集点。
对跟踪到的特征点，按照被追踪到的次数排序并依次选点；使用mask进行类似非极大抑制的方法，半径为30，去掉分部密集的点，使特征点分布均匀

void FeatureTracker::setMask()
{
    if(FISHEYE)
        mask = fisheye_mask.clone();
    else
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
    
    // prefer to keep features that are tracked for long time
    //构造(cnt，pts，id)序列
    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);
        }
    }
}

---

void FeatureTracker::rejectWithF()

该函数主要是通过基本矩阵（F）去除外点outliers。首先将将图像坐标畸变矫正后转换为像素坐标，通过cv::findFundamentalMat()计算F矩阵，利用得到的status通过reduceVector()去除outliers 。

void FeatureTracker::rejectWithF()
{
    if (forw_pts.size() >= 8)
    {
        ROS_DEBUG("FM ransac begins");
        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;
            
            //根据不同的相机模型将二维坐标转换到三维坐标
            //对于PINHOLE（针孔相机）可将像素坐标转换到归一化平面并去畸变
            //对于CATA（卡特鱼眼相机）将像素坐标投影到单位圆内
            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());
    }
}