VINS-Mono關鍵知識點總結——前端詳解

• VINS-Mono關鍵知識點總結——前端詳解
• • 1. VINS-Mono的前端流程概述
  • 2. setMask() 函數的作用
  • 3. rejectWithF() 函數的作用
  • 4. addPoints() 函數和 updataID() 函數的作用

VINS-Mono關鍵知識點總結——前端詳解

VINS-Mono的前端看上去思路是比較簡單的，但是如果仔細閱讀源碼的話還是能看出許多非常有技術性的東西的，下面我就把一些我覺得很有意思的細節碼出來，分享一下

1. VINS-Mono的前端流程概述

VINS-Mono的前端整個封裝成了一個ROS節點
其訂閱的topic是：

相機或者數據集發來的圖片

其發布topic是：

由pub_img發布的"feature"，發布的是當前幀的特征點，特征點分裝成了sensor_msgs::PointCloudPtr類型，里面包括了每個特征點的（1）歸一化平面上的坐標（2）歸一化平面上的速度（3）圖像平面上的坐標，這三者中歸一化平面坐標是在SFM和后端用到，而歸一化平面上的速度以及圖像平面上的坐標僅僅在進行時間偏差估計時會用到。

由pub_match發布的“feature_img”，發布的是用來調試的帶特征點的灰度圖，圖上特征點的顏色越紅代表此特征點被跟蹤的幀數越多，越藍表示越少。

在圖片的回調函數中會先對時間進行控制，然后會進入 readImage() 函數 進行光流追蹤、特征點提取、特征點提取等步驟

2. setMask() 函數的作用

setMask() 函數主要是和特征點提取有關的，當跟蹤的特征點數量沒有達到設定的值時就會對圖片進行特征點提取，提取采取的算法是OpenCV的Harris角點提取函數

cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);

函數的最后一個參數mask是是設置角點提取的區域，實際上是和輸入forw_img尺寸相同的一個Mat數據類型，而這個mask就在這里就是通過setMask()函數生成的，它的作用是使得提取的特征點分布盡可能均勻，setMask() 函數源碼如下：

void FeatureTracker::setMask() {if(FISHEYE)mask = fisheye_mask.clone();elsemask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));//算法會更加傾向于保留跟蹤時間長的特征點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);}} }

其大概流程是會根據光流法跟蹤下一幀圖片forw_img獲得的forw_pts的被跟蹤的時間進行排序，然后按照被跟蹤時間從長到段的順序以半徑MIN_DIST在一張白圖上畫黑圈，如果畫出來的mask大概長下面這個樣子（有點密集恐懼癥了…）：

將這個mask圖輸入到goodFeaturesToTrack函數中，函數就只會在白色區域，也就是沒有跟蹤時長特別長的地區提取特征點，因此會使得我們提取的關鍵點相對較為均勻，如下圖所示：

如果你將setMask函數中排序的部分注釋掉，特征點的分布就會變成如下圖所示結果：

我個人覺得上面這種做法應該是有利有弊的，對于紋理特征豐富的環境，這種操作使得特征點分布更加均勻是有利于系統魯棒性的，在ORB SLAM2和SVO等經典框架中，前端都有相應的均勻分布的操作，但是在上面圖片中這種紋理特征稀疏的環境下，這種做法就不一定好用了，因為強行使得特征點分布到一些本來就沒什么特征的區域，這樣可能（因為這里我還沒有做實驗驗證）會使得系統精度下降。

3. rejectWithF() 函數的作用

rejectWithF() 函數的源碼如下，其實比較簡單：

void FeatureTracker::rejectWithF() {if (forw_pts.size() >= 8){ROS_DEBUG("FM ransac begins");//才喲ing的是ransac的方法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());} }

這里主要是操作是將圖像平面上的特征點，投影到歸一化平面上然后消除畸變，這一步是通過liftProjective函數實現的，cameral_model功能包通過工廠模式生成了多種相機模型，不同的相機模型的liftProjective函數里面算法不同，但是好像這些相機模型對應的畸變消除算法和《視覺SLAM十四講》中講的最基本的那種$k_2,k_4,k_6,p_1,p_2$五個參數的都不同，具體的相機模型我還沒有時間去研究，這里我還有兩個問題：

為什么FOCAL_LENGTH在程序中設置成了460這個數值？

為什么要將特征帶點轉化到歸一化平面上的像素坐標？

排除這兩個問題之外，其他其實也就很簡單了，通過findFundamentalMat函數自帶的ransac消除外點的功能在光流法之后以及提取新的特征點之前消除光流法更蹤錯誤的點，維持系統精度。

4. addPoints() 函數和 updataID() 函數的作用

addPoints() 函數代碼如下：

void FeatureTracker::addPoints() {for (auto &p : n_pts){forw_pts.push_back(p);ids.push_back(-1);//新提取的特征點id初始化為-1track_cnt.push_back(1);//新提取的特征點被跟蹤的次數初始化為1} }

updataID() 函數代碼如下：

bool FeatureTracker::updateID(unsigned int i) {if (i < ids.size()){if (ids[i] == -1)ids[i] = n_id++;return true;}elsereturn false; }

這兩個函數都很短啊，addPoints() 函數主要是將goodFeaturesToTrack() 函數中提取的新的特征點n_pts加入到下一幀的關鍵點forw_pts中，注意這里將新添加的特征點的id都設為了-1， 而updataID() 函數在這之后將id從n_id開始賦值，而n_id是一個全局變量，從第一個特征點為1開始來一個特征點加一，因此是一個一直增長且不重復的整數，而我們特征點對應的id如果答應出來可以發現是一個亂序且不連續的數列，為什么呢？這可以結合到我們上面講的setMask() 函數和 rejectWithF() 函數，因為他們分別對這個數列進行了排序和刪除～

VINS的前端其實很簡單的，其精華主要是在后端的優化的邊緣化，前端就總結到這里啦，歡迎指正。

此外，對其他SLAM算法感興趣的同學可以看考我的博客SLAM算法總結——經典SLAM算法框架總結

總結

以上是生活随笔為你收集整理的VINS-Mono关键知识点总结——前端详解的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。