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點的一點補充，代碼：

可以根據不同的相機模型将二維坐标轉換到三維坐标：

對于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<cv::Point2f> n_pts;//每一幀中新提取的特征點
    vector<cv::Point2f> prev_pts, cur_pts, forw_pts;//對應的圖像特征點
    vector<cv::Point2f> prev_un_pts, cur_un_pts;//歸一化相機坐标系下的坐标
    vector<cv::Point2f> pts_velocity;//目前幀相對前一幀特征點沿x,y方向的像素移動速度
    vector<int> ids;//能夠被跟蹤到的特征點的id
    vector<int> track_cnt;//目前幀forw_img中每個特征點被追蹤的時間次數

    map<int, cv::Point2f> cur_un_pts_map;
    map<int, cv::Point2f> 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
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中的一些配置參數

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

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、判斷是否是第一幀

2、判斷時間間隔是否正确，有問題則restart

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() 函數讀取圖像資料進行處理

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());
    }
}