AI

人工智能

代碼解讀 | VINS 視覺前端

本文作者是計算機視覺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<:pointcloud>("feature", 1000);

pub_match = n.advertise<:image>("feature_img",1000);

pub_restart = n.advertise<: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<: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 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

推薦閱讀

內容來源于網絡，如有侵權請聯系客服刪除

總結

以上是生活随笔為你收集整理的vins 解读_代码解读 | VINS 视觉前端的全部內容，希望文章能夠幫你解決所遇到的問題。

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