代碼解讀

本文轉載自查看原文 2019-08-30 19:28 1059 計算機視覺/ 圖像拼接

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

vins前端概述

在搞清楚VINS前端之前，首先要搞清楚什么是SLAM前端？

SLAM的前端、后端系統本身沒有特別明確的划分，但是在實際研究中根據處理的先后順序一般認為特征點提取和跟蹤為前端部分，然后利用前端獲取的數據進行優化、回環檢測等操作，從而將優化、回環檢測等作為后端。

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

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，用來計時；
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
https://blog.csdn.net/wangshuailpp/article/details/78461171
https://blog.csdn.net/qq_41839222/article/details/85797156
https://qingsimon.github.io/post/
關注公眾號，點擊“學習圈子”，“SLAM入門“”，從零開始學習三維視覺核心技術SLAM，3天內無條件退款。早就是優勢，學習切忌單打獨斗，這里有教程資料、練習作業、答疑解惑等，優質學習圈幫你少走彎路，快速入門！

免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。

猜您在找 VINS-mono詳細解讀 VINS（一）簡介與代碼結構 VINS-Mono代碼分析與總結(完整版) VINS-MONO ProjectionFactor代碼分析及公式推導前端解讀Webview 怎樣解讀Caffe源代碼 cartographer 代碼解讀 EfficientNet代碼解讀筆記論文解讀|激光視覺融合的Ｖ-LOAM 以藍牙開發的視覺解讀微信Airsync協議

代碼解讀 | VINS 視覺前端