VINS FUSION入門系列特徵點的跟蹤

// 對進行一系列操作，返回特徵點featureframe。
// 其中還包含了：影象處理、區域mask、檢測特徵點、計算畫素速度等
map>>> featuretracker::trackimage(double _cur_time, const cv::mat &_img, const cv::mat &_img1)

當起始沒有特徵點的時候先就行特徵點的提取，下面**是先考慮的先前跟蹤的特徵點是否不小於設定的值，如果小於，則再用goodfeaturestotrack這個函式進行提取剩餘的點。

// 如果當前影象的特徵點cur_pts數目小於規定的最大特徵點數目max_cnt，則進行提取
int n_max_cnt = max_cnt - static_cast(cur_pts.size()); // 在影象上所需要提取的特徵點最大值
if (n_max_cnt > 0)
else
n_pts.clear();

一提到角點檢測，最常用的方法莫過於harris角點檢測，opencv中也提供了harris角點檢測的介面，即cv::cornerharris()，但是harris角點檢測存在很多缺陷（如角點是畫素級別的，速度較慢等），因此我們這裡將介紹opencv中的另乙個功能更為強大的函式——cv::goodfeaturestotrack()，它不僅支援harris角點檢測，也支援shi tomasi演算法的角點檢測。但是，該函式檢測到的角點依然是畫素級別的，若想獲取更為精細的角點座標，則需要呼叫cv::cornersubpix()函式進一步細化處理，即亞畫素。

void cv::goodfeaturestotrack(
cv::inputarray image, // 輸入影象（cv_8uc1 cv_32fc1）
cv::outputarray corners, // 輸出角點vector
int maxcorners, // 最大角點數目
double qualitylevel, // 質量水平係數（小於1.0的正數，一般在0.01-0.1之間）
double mindistance, // 最小距離，小於此距離的點忽略
cv::inputarray mask = noarray(), // mask=0的點忽略
int blocksize = 3, // 使用的鄰域數
bool useharrisdetector = false, // false ='shi tomasi metric'
double k = 0.04 // harris角點檢測時使用
);

第乙個引數是輸入影象（8位或32位單通道圖）。

第二個引數是檢測到的所有角點，型別為vector或陣列，由實際給定的引數型別而定。如果是vector，那麼它應該是乙個包含cv::point2f的vector物件；如果型別是cv::mat,那麼它的每一行對應乙個角點，點的x、y位置分別是兩列。

第三個引數用於限定檢測到的點數的最大值。

第四個引數表示檢測到的角點的質量水平（通常是0.10到0.01之間的數值，不能大於1.0）。

第五個引數用於區分相鄰兩個角點的最小距離（小於這個距離得點將進行合併）。

第六個引數是mask，如果指定，它的維度必須和輸入影象一致，且在mask值為0處不進行角點檢測。

第七個引數是blocksize，表示在計算角點時參與運算的區域大小，常用值為3，但是如果影象的解析度較高則可以考慮使用較大一點的值。

第八個引數用於指定角點檢測的方法，如果是true則使用harris角點檢測，false則使用shi tomasi演算法。

第九個引數是在使用harris演算法時使用，最好使用預設值0.04。

當先前已經有了特徵點以後，就可以直接跟蹤前面的特徵點。進行反向跟蹤主要是去除一些有問題的點。hasprediction是把上一幀3d點**到歸一化平面，**方法好像就是直接把3d點投影下來。

// --------------如果上一幀有特徵點，就直接進行lk追蹤
if (prev_pts.size() > 0) 
if (succ_num < 10)//小於10時，好像會擴大搜尋，輸入的基於最大金字塔層次數為3
cv::calcopticalflowpyrlk(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::size(21, 21), 3);
}else 
//如果沒有進行**的話，直接是基於最大金字塔層次數為3
cv::calcopticalflowpyrlk(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::size(21, 21), 3);
// reverse check 方向檢查
if(flow_back)//
else
status[i] = 0;}}
for (int i = 0; i < int(cur_pts.size()); i++)
if (status[i] && !inborder(cur_pts[i]))// 如果這個點不在影象內，則剔除
status[i] = 0;
reducevector(prev_pts, status);
reducevector(cur_pts, status);
reducevector(ids, status);
reducevector(track_cnt, status);
ros_debug("temporal optical flow costs: %fms", t_o.toc());
printf("track cnt %d\n", (int)ids.size());
}

畫出rviz上跟蹤的顯示效果

//在imtrack影象上畫出特徵點
void featuretracker::drawtrack(const cv::mat &imleft, const cv::mat &imright, 
vector&curleftids,
vector&curleftpts, 
vector&currightpts,
map&prevleftptsmap)
// -------------在右目影象上標記特徵點
if (!imright.empty() && stereo_cam)
}map::iterator mapit;
for (size_t i = 0; i < curleftids.size(); i++)
}}

當先前已經有跟蹤點了，然後需要在提取新的特徵點，在沒有提取特徵點的地方進行提取，先前提取了特徵的地方mask為０．

//todo: 這裡應該可以做動態檢測
// 把追蹤到的點進行標記
// 設定遮擋部分（魚眼相機）
// 對檢測到的特徵點按追蹤到的次數排序
// 在mask影象中將追蹤到點的地方設定為0，否則為255，目的是為了下面做特徵點檢測的時候可以選擇沒有特徵點的區域進行檢測。
// 在同一區域內，追蹤到次數最多的點會被保留，其他的點會被刪除
void featuretracker::setmask()
); cur_pts.clear();
ids.clear();
track_cnt.clear();
for (auto &it : cnt_pts_id)//對於所有追蹤到的點
// todo:這裡應該輸出一下mask 看看效果
// cv::imshow ( "mask", mask ); // 用cv::imshow顯示影象
// cv::waitkey ( 0 ); // 暫停程式,等待乙個按鍵輸入
}}

當如果是雙目的時候對左目右目的點進行跟蹤，找到雙目的匹配點，為後續的三角化和pnp求解位置做準備。

// ---------------如果是雙目的
if(!_img1.empty() && stereo_cam) 
// 把左目的點在右目上找到，然後計算右目上的畫素速度。
}ids_right = ids;
reducevector(cur_right_pts, status);
reducevector(ids_right, status);
// 僅保留左右目都有的點only keep left-right pts
/*reducevector(cur_pts, status);
reducevector(ids, status);
reducevector(track_cnt, status);
reducevector(cur_un_pts, status);
reducevector(pts_velocity, status);
*/cur_un_right_pts = undistortedpts(cur_right_pts, m_camera[1]);//右目的歸一化平面點
right_pts_velocity = ptsvelocity(ids_right, cur_un_right_pts, cur_un_right_pts_map, prev_un_right_pts_map);
}prev_un_right_pts_map = cur_un_right_pts_map;
}

這部分主要是為了利用ｌｋ光流找出相鄰兩幀的特徵點的匹配關係，為後續的求最小重投影誤差做準備，利用ｌｋ光流找出左右目之間的匹配關係，為後續的三角化做準備，總體來說這個過程相對與描述子的匹配耗時是少很多的。

VINS FUSION入門系列特徵點的跟蹤

斯特林數入門

PaddlePaddle入門之CV疫情特輯

設計模式系列迪公尺特法則

VINS FUSION入門系列 特徵點的跟蹤

斯特林數入門

PaddlePaddle入門之CV疫情特輯

設計模式系列 迪公尺特法則

相關推薦

VINS FUSION入門系列特徵點的跟蹤

設計模式系列迪公尺特法則