代碼解讀 | VINS 視覺前端

時間 2019-11-10

原文原文鏈接

本文做者是計算機視覺life公衆號成員蔡量力，因爲格式問題部份內容顯示可能有問題，更好的閱讀體驗，請查看原文連接：代碼解讀 | VINS 視覺前端前端

vins前端概述

在搞清楚VINS前端以前，首先要搞清楚什麼是SLAM前端？node

SLAM的前端、後端系統自己沒有特別明確的劃分，可是在實際研究中根據處理的前後順序通常認爲特徵點提取和跟蹤爲前端部分，而後利用前端獲取的數據進行優化、迴環檢測等操做，從而將優化、迴環檢測等做爲後端。git

而在VINS_MONO中將視覺跟蹤模塊（feature_trackers）爲其前端。在視覺跟蹤模塊中，首先，對於每一幅新圖像，KLT稀疏光流算法對現有特徵進行跟蹤。而後，檢測新的角點特徵以保證每一個圖像特徵的最小數目，並設置兩個相鄰特徵之間像素的最小間隔來執行均勻的特徵分佈。接着，將二維特徵點去畸變，而後在經過外點剔除後投影到一個單位球面上。最後，利用基本矩陣模型的RANSAC算法進行外點剔除。github

VINS_MONO原文中還將關鍵幀的選取做爲前端分，本文暫不討論，後續文章會詳細介紹。算法

VINS-Mono將前端封裝爲一個ROS節點，該節點的實如今feature_tracker目錄下的src中，src裏共有3個頭文件和3個源文件：編程

feature_tracker_node.cpp構造了一個ROS節點feature_tracker_node，該節點訂閱相機圖像話題數據後，提取特徵點，而後用KLT光流進行特徵點跟蹤。feature_tracker節點將跟蹤的特徵點做爲話題進行發佈，供後端ROS節點使用。同時feature_tracker_node還會發布標記了特徵點的圖片，可供Rviz顯示以供調試。以下表所示:後端

操做	話題	消息類型	功能
Subscribe	image	sensor_msgs::ImageConstPtr	訂閱原始圖像，傳給回調函數
Publish	feature	sensor_msgs::PointCloud	跟蹤的特徵點，供後端優化使用
Publish	feature_img	sensor_msgs::Image	跟蹤特徵點圖片，輸出給RVIZ，調試用

feature_tracker.h和feature_tracker.cpp實現了一個類FeatureTracker，用來完成特徵點提取和特徵點跟蹤等主要功能，該類中主要函數和實現的功能以下：函數

函數	功能
bool inBorder()	判斷跟蹤的特徵點是否在圖像邊界內
void reduceVector()	去除沒法跟蹤的特徵點
void FeatureTracker::setMask()	對跟蹤點進行排序並去除密集點
void FeatureTracker::addPoints()	添將新檢測到的特徵點n_pts
void FeatureTracker::readImage()	對圖像使用光流法進行特徵點跟蹤
void FeatureTracker::rejectWithF()	利用F矩陣剔除外點
bool FeatureTracker::updateID()	更新特徵點id
void FeatureTracker::readIntrinsicParameter()	讀取相機內參
void FeatureTracker::showUndistortion()	顯示去畸變矯正後的特徵點
void FeatureTracker::undistortedPoints()	對角點進行去畸變矯正，並計算每一個角點的速度

tic_toc.h中是做者本身封裝的一個類TIC_TOC，用來計時；post
parameters.h和parameters.cpp處理前端中須要用到的一些參數；學習

流程圖

代碼解讀

feature_tracker_node系統入口main() 函數：

ROS初始化和輸出調試信息：

//ros初始化和設置句柄
ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");
//設置logger的級別。 只有級別大於或等於level的日誌記錄消息纔會獲得處理。
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

讀取配置參數：

//讀取config->euroc->euroc_config.yaml中的一些配置參數
readParameters(n);

讀取相機內參讀取每一個相機對應內參，單目時NUM_OF_CAM=1：

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

判斷是否加入魚眼mask來去除邊緣噪聲
訂閱話題IMAGE_TOPIC，當有圖像進來的時候執行回調函數：
```
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
```

將處理完的圖像信息用PointCloud實例feature_points和Image的實例ptr消息類型，發佈到"feature"和"feature_img"的topic

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

回調函數imf_callback

判斷是否爲第一幀，若爲第一幀，將該幀的時間賦給 first_image_time和last_image_time ,而後返回

if(first_image_flag)
    {
        first_image_flag = false;
        first_image_time = img_msg->header.stamp.toSec();//記錄圖像幀的時間
        last_image_time = img_msg->header.stamp.toSec();
        return;
    }

經過判斷時間間隔，有問題則restart

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

發佈頻率控制（不是每來一張圖像都要發佈，可是都要傳入readImage()進行處理），保證每秒鐘處理的圖像不超過FREQ，此處爲每秒10幀

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;

將圖像編碼8UC1轉換爲mono8
處理圖片：readImage()
判斷是否顯示去畸變矯正後的特徵點

更新全局ID，將新提取的特徵點賦予全局id

for (unsigned int i = 0;; i++)
{
    bool completed = false;
    for (int j = 0; j < NUM_OF_CAM; j++)
        if (j != 1 || !STEREO_TRACK)
            completed |= trackerData[j].updateID(i);
    if (!completed)
        break;
}

將特徵點id，矯正後歸一化平面的3D點(x,y,z=1)，像素2D點(u,v)，像素的速度(vx,vy)，封裝成sensor_msgs::PointCloudPtr類型的feature_points實例中，發佈到pub_img，將圖像封裝到cv_bridge::cvtColor類型的ptr實例中發佈到pub_match
發佈消息的數據：

pub_img.publish(feature_points)

pub_match.publish(ptr->toImageMsg())

readimage()

判斷EQUALIZE的值，決定是否對圖像進行直方圖均衡化處理：createCLAHE()
若爲第一次讀入圖片，則：prev_img = cur_img = forw_img = img；若不是第一幀，則：forw_img = img，其中cur_img 和 forw_img 分別是光流跟蹤的先後兩幀，forw_img 纔是真正的當前幀，cur_img 其實是上一幀，prev_img 是上一次發佈的幀。
```
prev_img = cur_img = forw_img = img;//避免後面使用到這些數據時，它們是空的
```
調用 cv::calcOpticalFlowPyrLK()進行光流跟蹤，跟蹤前一幀的特徵點 cur_pts 獲得 forw_pts，根據 status 把跟蹤失敗的點剔除(注意 prev, cur, forw, ids, track_cnt都要剔除)，並且還須要將跟蹤到圖像邊界外的點剔除。
```
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
```

判斷是否須要發佈該幀圖像：

否(PUB_THIS_FRAME=0)：當前幀 forw 的數據賦給上一幀 cur，而後在這一步就結束了。

是(PUB_THIS_FRAME=0)：

調用rejectWithF()對prev_pts和forw_pts作RANSAC剔除outlier，函數裏面主要是調用了cv::findFundamentalMat() 函數，而後將而後全部剩下的特徵點的 track_cnt 加1，track_cnt數值越大，說明被追蹤得越久。

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;
            //根據不一樣的相機模型將二維座標轉換到三維座標
            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());
    }
}

調用setMask()函數，先對跟蹤到的特徵點 forw_pts 按照跟蹤次數降序排列(認爲特徵點被跟蹤到的次數越多越好)，而後遍歷這個降序排列，對於遍歷的每個特徵點，在 mask中將該點周圍半徑爲 MIN_DIST=30 的區域設置爲 0，在後續的遍歷過程當中，再也不選擇該區域內的點。
在mask中不爲0的區域，調用goodFeaturesToTrack提取新的角點n_pts，經過addPoints()函數push到forw_pts中，id初始化-1，track_cnt初始化爲1（因爲跟蹤過程當中，上一幀特徵點因爲各類緣由沒法被跟蹤，並且爲了保證特徵點均勻分佈而剔除了一些特徵點，若是不補充新的特徵點，那麼每一幀中特徵點的數量會愈來愈少）。
```
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
```

調用undistortedPoints() 函數根據不一樣的相機模型進行去畸變矯正和深度歸一化，計算速度。

reference

https://github.com/QingSimon/VINS-Mono-code-annotation/blob/master/VINS-Mono%E8%AF%A6%E8%A7%A3.pdf
http://www.javashuo.com/article/p-xkgyhfoi-de.html
http://www.javashuo.com/article/p-bjifdxzf-nm.html
https://qingsimon.github.io/post/
關注公衆號，點擊「學習圈子」，「SLAM入門「」，從零開始學習三維視覺核心技術SLAM，3天內無條件退款。早就是優點，學習切忌單打獨鬥，這裏有教程資料、練習做業、答疑解惑等，優質學習圈幫你少走彎路，快速入門！