2. 程式人生 > >程式碼解讀 | VINS 視覺前端

程式碼解讀 | VINS 視覺前端

阿新 • • 發佈：2019-08-30

本文作者是計算機視覺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() 函式：

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

  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，當有影象進來的時候執行回撥函式：

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

  6. 將處理完的影象資訊用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

  1. 判斷是否為第一幀，若為第一幀，將該幀的時間賦給 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;
    }

  2. 通過判斷時間間隔，有問題則restart

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

  3. 釋出頻率控制（不是每來一張影象都要釋出，但是都要傳入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;

  4. 將影象編碼8UC1轉換為mono8

  5. 處理圖片：readImage()

  6. 判斷是否顯示去畸變矯正後的特徵點

  7. 更新全域性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;
}

  8. 將特徵點id，矯正後歸一化平面的3D點(x,y,z=1)，畫素2D點(u,v)，畫素的速度(vx,vy)，封裝成sensor_msgs::PointCloudPtr型別的feature_points例項中，釋出到pub_img，將影象封裝到cv_bridge::cvtColor型別的ptr例項中釋出到pub_match

  9. 釋出訊息的資料：

    pub_img.publish(feature_points)

    pub_match.publish(ptr->toImageMsg())

readimage()

  1. 判斷EQUALIZE的值，決定是否對影象進行直方圖均衡化處理：createCLAHE()

  2. 若為第一次讀入圖片，則：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;//避免後面使用到這些資料時，它們是空的

  3. 呼叫 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);

  4. 判斷是否需要釋出該幀影象：

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

    是(PUB_THIS_FRAME=0)：

    1. 呼叫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());
    }
}

    2. 呼叫setMask()函式，先對跟蹤到的特徵點 forw_pts 按照跟蹤次數降序排列(認為特徵點被跟蹤到的次數越多越好)，然後遍歷這個降序排列，對於遍歷的每一個特徵點，在 mask中將該點周圍半徑為 MIN_DIST=30 的區域設定為 0，在後續的遍歷過程中，不再選擇該區域內的點。

    3. 在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);

  5. 呼叫undistortedPoints() 函式根據不同的相機模型進行去畸變矯正和深度歸一化，計算速度。

reference

  1. https://github.com/QingSimon/VINS-Mono-code-annotation/blob/master/VINS-Mono%E8%AF%A6%E8%A7%A3.pdf

  2. https://blog.csdn.net/wangshuailpp/article/details/78461171

  3. https://blog.csdn.net/qq_41839222/article/details/85797156

  4. https://qingsimon.github.io/post/
    關注公眾號，點選“學習圈子”，“SLAM入門“”，從零開始學習三維視覺核心技術SLAM，3天內無條件退款。早就是優勢，學習切忌單打獨鬥，這裡有教程資料、練習作業、答疑解惑等，優質學習圈幫你少走彎路，快速入門！

推薦閱讀

如何從零開始系統化學習視覺SLAM？
從零開始一起學習SLAM | 為什麼要學SLAM？
從零開始一起學習SLAM | 學習SLAM到底需要學什麼？
從零開始一起學習SLAM | SLAM有什麼用？
從零開始一起學習SLAM | C++新特性要不要學？
從零開始一起學習SLAM | 為什麼要用齊次座標？
從零開始一起學習SLAM | 三維空間剛體的旋轉
從零開始一起學習SLAM | 為啥需要李群與李代數？
從零開始一起學習SLAM | 相機成像模型
從零開始一起學習SLAM | 不推公式，如何真正理解對極約束?
從零開始一起學習SLAM | 神奇的單應矩陣
從零開始一起學習SLAM | 你好，點雲
從零開始一起學習SLAM | 給點雲加個濾網
從零開始一起學習SLAM | 點雲平滑法線估計
從零開始一起學習SLAM | 點雲到網格的進化
從零開始一起學習SLAM | 理解圖優化，一步步帶你看懂g2o程式碼
從零開始一起學習SLAM | 掌握g2o頂點程式設計套路
從零開始一起學習SLAM | 掌握g2o邊的程式碼套路
從零開始一起學習SLAM | 用四元數插值來對齊IMU和影象幀
零基礎小白，如何入門計算機視覺？
SLAM領域牛人、牛實驗室、牛研究成果梳理
我用MATLAB擼了一個2D LiDAR SLAM
視覺化理解四元數，願你不再掉頭髮
最近一年語義SLAM有哪些代表性工作？
視覺SLAM技術綜述
彙總 | VIO、鐳射SLAM相關論文分類集錦
研究SLAM，對程式設計的要求有多高？
2018年SLAM、三維視覺方向求職經驗分享
2018年SLAM、三維視覺方向求職經驗分享
深度學習遇到SLAM | 如何評價基於深度學習的DeepVO，VINet，VidLoc？
AI資源對接需求彙總：第1期
AI資源對接需求彙總：第2期
AI資源對接需求彙總：第3

相關推薦

程式碼解讀 | VINS 視覺前端

本文作者是計算機視覺life公眾號成員蔡量力，由於格式問題部分內容顯示可能有問題，更好的閱讀體驗，請檢視原文連結：程式碼解讀 | VINS 視覺前端 vins前端概述 在搞清楚VINS前端之前，首先要搞清楚什麼是SLAM前端？ SLAM的前端、後端系統本身沒有特別明確的劃分，但是在實際研究中根據處理的先後順序

VINS-Mono程式碼解讀——視覺跟蹤 feature_trackers

前言 本文主要介紹VINS的視覺處理前端的視覺跟蹤模組（feature_trackers）。 論文第四章A節（IV. MEASUREMENT PREPROCESSING——A. Vision Processing Front-end） 可以看到其演算法主要包括以下內容： 1、對於每一幅新

VINS-Mono程式碼解讀——狀態估計器流程 estimator

前言 本文主要介紹VINS的狀態估計器模組（estimator），從ROS角度說是estimator節點。 這個模組可以說是VINS的最核心模組，從論文的內容上來說，裡面的內容包括了VINS的估計器初始化、基於滑動視窗的非線性優化實現緊耦合，即論文第五章（V. ESTIMATOR INI

VINS-Mono程式碼解讀——各種資料結構 sensor_msgs

前言 在看VINS-Mono的程式碼時，覺得非常有必要整理總結一下其中不同的資料結構，尤其是各種sensor_msgs的格式與具體含義。慢慢補充吧！ 標準msg結構 1、std_msgs/Header from file：std_msgs/Header.msg 一般在I

第一個python小程式——即時動態時鐘(程式碼解讀)

程式碼資源來自： http://n.miaopai.com/media/K9Qlou7rdPc5TxpPaL1VDvwfv5hP~lHK   (執行有錯誤，缺少date，week和結尾部分) https://blog.csdn.net/yangxing2/article/details

freeModbus程式碼解讀及移植筆記

分享一下我老師大神的人工智慧教程！零基礎，通俗易懂！http://blog.csdn.net/jiangjunshow 也歡迎大家轉載本篇文章。分享知識，造福人民，實現我們中華民族偉大復興！        

[arduino]-1-Basics程式碼解讀

arduino IDE 自帶示例中的Basics系列， 可以說是 向一個擁有C系家族程式語言基礎的人 解釋清楚了它自己的基本操作 BareMinimum（基本檔案結構）： void setup() { // put your setup code here, to run once:

colmap程式碼解讀

clomap是作者在ECCV2016年發表的基於兩個概率的深度值和法線估計的論文（開源），下面就開原始碼Patch_match_cuda.cu檔案做簡單的介紹   產生隨機法向量和隨機深度值 擾動法向量（產生隨機三個方位角度）和擾動深度值 根據畫素座標計算三維空間

【PHP】靜態方法呼叫非靜態方法和靜態呼叫非靜態方法程式碼解讀

static 關鍵字用來修飾屬性、方法，稱這些屬性、方法為靜態屬性、靜態方法。 在類的靜態方法中是不能直接以$this->test()的方式呼叫非靜態方法的。還有框架中靜態的呼叫非靜態方法是怎麼回事？？？ 。。。 算了，不知道說啥 具體為啥看程式碼註釋： <?ph

RPN網路程式碼解讀

1. 說在前面的話 在目標檢測領域Faster RCNN可以說是無人不知無人不曉，它裡面有一個網路結構RPN（Region Proposal Network）用於在特徵圖上產生候選預測區域。但是呢，這個網路結構具體是怎麼工作的呢？網上有很多種解釋，但是都是雲裡霧裡的，還是直接擼程式碼

Kubernetes(k8s)程式碼解讀

apiserver的list-watch程式碼解讀 list-watch，作為k8s系統中統一的非同步訊息傳遞方式，對系統的效能、資料一致性起到關鍵性的作用。今天我想從程式碼這邊探究一下list-watch的實現方式。並看是否能在後面的工作中優化這個過程。 0. list-watch的需求

mxnet 資料部分介面解讀和視覺化

（一）資料讀取 這裡主要用到兩個函式介面：io.ImageRecordIter以及recordio.MXIndexedRecordIO。 io.ImageRecordIter： 函式解釋：從rec檔案中讀取資料，根據batchSize的大小讀取資料。 函式引數：（幾

實現servlet介面並優化程式碼解讀

3.2.1 方式一：直接實現類實現servlet介面，並全部重寫servlet介面中的方法 實現類： //Servlet 介面是用來獲取瀏覽器請求（request）的資料，以及將處理完的資料響應（response）給瀏覽器 public class ServletDe

神經網路中反向傳播演算法(backpropagation)的pytorch實現，pytorch教程中的程式碼解讀以及其他一些疑問與解答

pytorch的官網上有一段教程，是使用python的numpy工具實現一個簡單的神經網路的bp演算法。下面先貼上自己的程式碼： import numpy as np N,D_in,H,D_out = 4,10,8,5 x = np.random.randn(N,D_i

平衡小車之家PC端上位機協議程式碼解讀

平臺是基於STM32F103C8T6，資料可從這裡下載連結：連結，提取碼：qskh 一、程式碼再現 DataScope_DP.c #include "DataScope_DP.h" unsigned char DataScope_OutPut_Buffer[4

kubernetes原始碼版本1.2 程式碼解讀

kubernetes原始碼版本1.2.0 （目前最新kubernetes版本1.4） 程式碼閱讀方法 先簡單講講整個程式碼的目錄結構 | 目錄 | 說明 | | ———– | —————————————- | | api | 輸出介面文件用 | | build | 構建指令碼 | | cmd

騰訊開源的Paxos庫PhxPaxos程式碼解讀---Prepare階段(一)

簡單的畫了一下PhxPaxos在Prepare階段的邏輯,主要是正常的邏輯,異常邏輯和超時後面再寫了;   熟悉PhxPaxos程式碼最好的方法是編譯執行sample目錄下的三個例子,編譯方法在另一篇部落格已經寫了,這裡忽略;    　　　　一: Proposor在Pre

Android MVP模式 谷歌官方程式碼解讀

Google官方MVP Sample程式碼解讀 關於Android程式的構架, 當前(2016.10)最流行的模式即為MVP模式, Google官方提供了Sample程式碼來展示這種模式的用法. Repo地址: android-architecture. 本文為閱讀官方sample程式碼的閱讀筆記和分析. 官

PSPNet測試程式碼解讀

PSPNet測試程式碼在原文《Pyramid Scene Parsing Network》作者的GitHub網站上https://github.com/hszhao/PSPNet，下載下來解壓後找到evaluation資料夾，裡面的六個.m檔案（Matlab程式碼）就是測試時用的程式碼，如下圖所示：

開源評測系統hustoj-程式碼解讀 及 快速安裝

/* * Copyright 2008 sempr <[email protected]> * * Refacted and modified by zhblue<[email protected]> * Bug report email [email&