AI

人工智能

代碼解讀 | VINS 視覺前端

本文作者是計(jì)算機(jī)視覺life公眾號(hào)成員蔡量力，由于格式問題部分內(nèi)容顯示可能有問題，更好的閱讀體驗(yàn)，請(qǐng)查看原文鏈接：代碼解讀 | VINS 視覺前端

vins前端概述

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

SLAM的前端、后端系統(tǒng)本身沒有特別明確的劃分，但是在實(shí)際研究中根據(jù)處理的先后順序一般認(rèn)為特征點(diǎn)提取和跟蹤為前端部分，然后利用前端獲取的數(shù)據(jù)進(jìn)行優(yōu)化、回環(huán)檢測(cè)等操作，從而將優(yōu)化、回環(huán)檢測(cè)等作為后端。

而在VINS_MONO中將視覺跟蹤模塊(feature_trackers)為其前端。在視覺跟蹤模塊中，首先，對(duì)于每一幅新圖像，KLT稀疏光流算法對(duì)現(xiàn)有特征進(jìn)行跟蹤。然后，檢測(cè)新的角點(diǎn)特征以保證每個(gè)圖像特征的最小數(shù)目，并設(shè)置兩個(gè)相鄰特征之間像素的最小間隔來執(zhí)行均勻的特征分布。接著，將二維特征點(diǎn)去畸變，然后在通過外點(diǎn)剔除后投影到一個(gè)單位球面上。最后，利用基本矩陣模型的RANSAC算法進(jìn)行外點(diǎn)剔除。

VINS_MONO原文中還將關(guān)鍵幀的選取作為前端分，本文暫不討論， 后續(xù)文章會(huì)詳細(xì)介紹。

VINS-Mono將前端封裝為一個(gè)ROS節(jié)點(diǎn)，該節(jié)點(diǎn)的實(shí)現(xiàn)在feature_tracker目錄下的src中，src里共有3個(gè)頭文件和3個(gè)源文件：

feature_tracker_node.cpp構(gòu)造了一個(gè)ROS節(jié)點(diǎn)feature_tracker_node，該節(jié)點(diǎn)訂閱相機(jī)圖像話題數(shù)據(jù)后，提取特征點(diǎn)，然后用KLT光流進(jìn)行特征點(diǎn)跟蹤。feature_tracker節(jié)點(diǎn)將跟蹤的特征點(diǎn)作為話題進(jìn)行發(fā)布，供后端ROS節(jié)點(diǎn)使用。同時(shí)feature_tracker_node還會(huì)發(fā)布標(biāo)記了特征點(diǎn)的圖片，可供Rviz顯示以供調(diào)試。如下表所示:

操作

話題

消息類型

功能Subscribe

image

sensor_msgs::ImageConstPtr

訂閱原始圖像，傳給回調(diào)函數(shù)

Publish

feature

sensor_msgs::PointCloud

跟蹤的特征點(diǎn)，供后端優(yōu)化使用

Publish

feature_img

sensor_msgs::Image

跟蹤特征點(diǎn)圖片，輸出給RVIZ，調(diào)試用

feature_tracker.h和feature_tracker.cpp實(shí)現(xiàn)了一個(gè)類FeatureTracker，用來完成特征點(diǎn)提取和特征點(diǎn)跟蹤等主要功能，該類中主要函數(shù)和實(shí)現(xiàn)的功能如下：

函數(shù)

功能bool inBorder()

判斷跟蹤的特征點(diǎn)是否在圖像邊界內(nèi)

void reduceVector()

去除無法跟蹤的特征點(diǎn)

void FeatureTracker::setMask()

對(duì)跟蹤點(diǎn)進(jìn)行排序并去除密集點(diǎn)

void FeatureTracker::addPoints()

添將新檢測(cè)到的特征點(diǎn)n_pts

void FeatureTracker::readImage()

對(duì)圖像使用光流法進(jìn)行特征點(diǎn)跟蹤

void FeatureTracker::rejectWithF()

利用F矩陣剔除外點(diǎn)

bool FeatureTracker::updateID()

更新特征點(diǎn)id

void FeatureTracker::readIntrinsicParameter()

讀取相機(jī)內(nèi)參

void FeatureTracker::showUndistortion()

顯示去畸變矯正后的特征點(diǎn)

void FeatureTracker::undistortedPoints()

對(duì)角點(diǎn)進(jìn)行去畸變矯正，并計(jì)算每個(gè)角點(diǎn)的速度

tic_toc.h中是作者自己封裝的一個(gè)類TIC_TOC，用來計(jì)時(shí)；

parameters.h和parameters.cpp處理前端中需要用到的一些參數(shù)；

流程圖

代碼解讀

feature_tracker_node系統(tǒng)入口main() 函數(shù)：

ROS初始化和輸出調(diào)試信息：

//ros初始化和設(shè)置句柄

ros::init(argc, argv, "feature_tracker");

ros::NodeHandle n("~");

//設(shè)置logger的級(jí)別。 只有級(jí)別大于或等于level的日志記錄消息才會(huì)得到處理。

ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

讀取配置參數(shù)：

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

readParameters(n);

讀取相機(jī)內(nèi)參讀取每個(gè)相機(jī)對(duì)應(yīng)內(nèi)參，單目時(shí)NUM_OF_CAM=1：

for (int i = 0; i < NUM_OF_CAM; i++)

trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);

判斷是否加入魚眼mask來去除邊緣噪聲

訂閱話題IMAGE_TOPIC，當(dāng)有圖像進(jìn)來的時(shí)候執(zhí)行回調(diào)函數(shù)：

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

將處理完的圖像信息用PointCloud實(shí)例feature_points和Image的實(shí)例ptr消息類型，發(fā)布到"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);

回調(diào)函數(shù)imf_callback

判斷是否為第一幀，若為第一幀，將該幀的時(shí)間賦給 first_image_time和last_image_time ,然后返回

if(first_image_flag)

{

first_image_flag = false;

first_image_time = img_msg->header.stamp.toSec();//記錄圖像幀的時(shí)間

last_image_time = img_msg->header.stamp.toSec();

return;

}

通過判斷時(shí)間間隔，有問題則restart

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

發(fā)布頻率控制(不是每來一張圖像都要發(fā)布，但是都要傳入readImage()進(jìn)行處理)，保證每秒鐘處理的圖像不超過FREQ，此處為每秒10幀

if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)

{

PUB_THIS_FRAME = true;

// 時(shí)間間隔內(nèi)的發(fā)布頻率十分接近設(shè)定頻率時(shí)，更新時(shí)間間隔起始時(shí)刻，并將數(shù)據(jù)發(fā)布次數(shù)置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轉(zhuǎn)換為mono8

處理圖片：readImage()

判斷是否顯示去畸變矯正后的特征點(diǎn)

更新全局ID，將新提取的特征點(diǎn)賦予全局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;

}

將特征點(diǎn)id，矯正后歸一化平面的3D點(diǎn)(x,y,z=1)，像素2D點(diǎn)(u,v)，像素的速度(vx,vy)，封裝成sensor_msgs::PointCloudPtr類型的feature_points實(shí)例中，發(fā)布到pub_img，將圖像封裝到cv_bridge::cvtColor類型的ptr實(shí)例中發(fā)布到pub_match

發(fā)布消息的數(shù)據(jù)：

pub_img.publish(feature_points)

pub_match.publish(ptr->toImageMsg())

readimage()

判斷EQUALIZE的值，決定是否對(duì)圖像進(jìn)行直方圖均衡化處理：createCLAHE()

若為第一次讀入圖片，則：prev_img = cur_img = forw_img = img；若不是第一幀，則：forw_img = img，其中cur_img 和 forw_img 分別是光流跟蹤的前后兩幀，forw_img 才是真正的當(dāng)前幀，cur_img 實(shí)際上是上一幀，prev_img 是上一次發(fā)布的幀。

prev_img = cur_img = forw_img = img;//避免后面使用到這些數(shù)據(jù)時(shí)，它們是空的

調(diào)用 cv::calcOpticalFlowPyrLK()進(jìn)行光流跟蹤，跟蹤前一幀的特征點(diǎn) cur_pts 得到 forw_pts，根據(jù) status 把跟蹤失敗的點(diǎn)剔除(注意 prev, cur, forw, ids, track_cnt都要剔除)，而且還需要將跟蹤到圖像邊界外的點(diǎn)剔除。

cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

判斷是否需要發(fā)布該幀圖像：

否(PUB_THIS_FRAME=0)：當(dāng)前幀 forw 的數(shù)據(jù)賦給上一幀 cur，然后在這一步就結(jié)束了。

是(PUB_THIS_FRAME=0)：

調(diào)用rejectWithF()對(duì)prev_pts和forw_pts做RANSAC剔除outlier，函數(shù)里面主要是調(diào)用了cv::findFundamentalMat() 函數(shù)，然后將然后所有剩下的特征點(diǎn)的 track_cnt 加1，track_cnt數(shù)值越大，說明被追蹤得越久。

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;

//根據(jù)不同的相機(jī)模型將二維坐標(biāo)轉(zhuǎn)換到三維坐標(biāo)

m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);

//轉(zhuǎn)換為歸一化像素坐標(biāo)

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;

//調(diào)用cv::findFundamentalMat對(duì)un_cur_pts和un_forw_pts計(jì)算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());

}

}

調(diào)用setMask()函數(shù)，先對(duì)跟蹤到的特征點(diǎn) forw_pts 按照跟蹤次數(shù)降序排列(認(rèn)為特征點(diǎn)被跟蹤到的次數(shù)越多越好)，然后遍歷這個(gè)降序排列，對(duì)于遍歷的每一個(gè)特征點(diǎn)，在 mask中將該點(diǎn)周圍半徑為 MIN_DIST=30 的區(qū)域設(shè)置為 0，在后續(xù)的遍歷過程中，不再選擇該區(qū)域內(nèi)的點(diǎn)。

在mask中不為0的區(qū)域，調(diào)用goodFeaturesToTrack提取新的角點(diǎn)n_pts，通過addPoints()函數(shù)push到forw_pts中，id初始化-1，track_cnt初始化為1(由于跟蹤過程中，上一幀特征點(diǎn)由于各種原因無法被跟蹤，而且為了保證特征點(diǎn)均勻分布而剔除了一些特征點(diǎn)，如果不補(bǔ)充新的特征點(diǎn)，那么每一幀中特征點(diǎn)的數(shù)量會(huì)越來越少)。

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

調(diào)用undistortedPoints() 函數(shù)根據(jù)不同的相機(jī)模型進(jìn)行去畸變矯正和深度歸一化，計(jì)算速度。

reference

推薦閱讀

內(nèi)容來源于網(wǎng)絡(luò)，如有侵權(quán)請(qǐng)聯(lián)系客服刪除

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。