VIN1.視覺前端

0.引言

搬運工+自己閱讀代碼理解。

參考說明

pipeline如圖：

本節則是閱讀總結Camera(30hz)-->Feature Detection and Tracking部分。主程序入口：feature_tracker_node.cpp;視覺跟蹤：feature_tracker.cpp.

1.流程

每個相機都有一個FeatureTracker實例，即TrackeData[i]，然後調用每個相機實例中的readImage()函數提取和跟蹤特徵點，然後將所有相機的特徵點融合到feature_points(sensor_msgs::PointCloudPtr)中發佈。FeatureTracker 類中最主要的成員函數是 readImage(),這裏涉及到圖像的3個img: prev_img 、cur_img、forw_img。cur_img 和 forw_img 分別是光流跟蹤的前後兩幀,forw_img 纔是真正的當前幀,cur_img 實際上是上一幀,prev_img 是上一次發佈的幀。prev_img 的用處是:光流跟蹤後用 prev_img 和 forw_img 根據 Fundamental Matrix 做 RANSAC 剔除 outlier,也就是rejectWithF()函數。代碼中NUM_OF_CAM爲單雙目標誌，NUM_OF_CAM=1單目。

.　　首先用cv::goodFeaturesToTrack在第一幀圖像上面找最強的MAX_CNT=150個特徵點，非極大值抑制半徑爲MIN_DIST=30。新的特徵點都有自己的新的對應的id。然後在下一幀過來時，對這些特徵點用光流法進行跟蹤，在下一幀上找匹配點。然後對前後幀中這些匹配點進行校正。先對特徵點進行畸變校正，再投影到以原點爲球心，半徑爲1的球面上，再延伸到深度歸一化平面上，獲得最終校正後的位置。對於每對匹配點，基於校正後的位置，用F矩陣加ransac來篩選。然後再在匹配上的特徵點之外的區域，用cv::goodFeaturesToTrack搜索最強的新的特徵點，把特徵點數量補上150個。
　　最後，把剩下的這些特徵點，把圖像點投影回深度歸一化平面上liftProjective()（對於這個函數我也沒弄明白，查了一下，參考,目前還沒去細看），再畸變校正，再投影到球面上，再延伸到深度歸一化平面上，得到校正後的位置。把校正後的位置發送出去（feature_points）。
　　特徵點跟蹤和匹配，就是前一幀到這一幀的，一幀幀繼承下去。或者生成新的特徵點。

1.1.readImage()函數

readImage()的作用是對新來的圖像使用光流法進行特徵點跟蹤,處理流程爲:

1.若控制參數 EQUALIZE 爲真,則調用 cv::creatCLAHE()對輸入圖像做自適應直方圖均衡;否則,不做處理。

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

3.如果不需要發佈當前幀的數據,那麼直接把當前幀 forw 的數據賦給上一幀 cur,然後在這一步就結束。

4.如果需要發佈當前幀的數據,先調用 rejectWithF()對 prev_pts 和 forw_pts 做RANSAC 剔除 outlier (調用 cv::findFundamentalMat()函數)。然後所有剩下的特徵點的 track_cnt 加 1。

5.在跟蹤過程中,爲了保持跟蹤到的特徵點在當前幀圖像中均勻分佈(避免特徵點扎堆的現象),會調用 FeatureTracker 類中的FeatureTracker:;setMask()函數,先對跟蹤到的特徵點 forw_pts 按照跟蹤次數降序排列(認爲特徵點被跟蹤到的次數越多越好),然後遍歷這個降序排列,對於遍歷的每一個特徵點,在 mask中將該點周圍半徑爲 MIN_DIST (30,30個像素周圍內不再提取特徵點)區域設置爲 0,在後續的遍歷過程中,不再選擇該區域內的點。

6.由於跟蹤過程中,上一幀特徵點由於各種原因無法被跟蹤,而且爲了保證特徵點均勻分佈而剔除了一些特徵點,如果不補充新的特徵點,那麼每一幀中特徵點的數量會越來越少。所以,當前幀除了跟蹤前一幀中的特徵點,還會調用cv::goodFeaturesToTrack()在 mask 中不爲 0 的區域提取新的特徵點：cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);新提取特徵點個數設置爲 MAX_CNT - forw_pts.size()個，MAX_CNT爲150即每幀提取150個特徵點。新提取的特徵點通過 FeatureTracker::addPoints()函數 push 到 forw_pts 中,id 初始化爲-1,track_cnt 初始化爲 1。

借個圖：

2.主要視覺函數

前端很多都是直接調用的opencv庫函數。

2.1.包含的視覺算法

（1）CLAHE(Contrast Limited Adaptive Histogram Equalization)

cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));

（2）Optical Flow(光流追蹤)參考1;參考2

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

（3）根據匹配點計算Fundamental Matrix，然後用Ransac剔除不符合Fundamental Matrix的外點

cv::findFundamentalMat(un_prev_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);

（4）特徵點檢測：goodFeaturesToTrack，使用Shi-Tomasi的改進版Harris corner

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

（5）特徵點排序：sort()，對光流跟蹤到的特徵點forw_pts，按照被跟蹤到的次數從大到小排序

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;     //xxx.first爲track_cnt[i]跟蹤次數
    });
    
//遍歷cnt_pts_id，構造mask，對關鍵點周圍的點不再提取關鍵點
for (auto &it : cnt_pts_id)
{
    if (mask.at<uchar>(it.second.first) == 255)
    {
        //當前特徵點位置對應的mask值爲255，則保留當前特徵點，將對應的特徵點位置，id，被追蹤次數分別存入forw_pts, ids, track_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);
    }
}

(6) liftProjective()函數

根據不同的相機模型將二維座標轉換到三維座標：
對於CATA（卡特魚眼相機）將像素座標投影到單位圓內，這裏涉及了魚眼相機模型；
而對於PINHOLE（針孔相機）將像素座標直接轉換到歸一化平面（z=1）並採用逆畸變模型(k1,k2,p1,p2)去畸變等。

特徵點之間保證了最小距離30個像素，跟蹤成功的特徵點需要經過rotation-compensated旋轉補償的視差計算，視差在30個像素以上的特徵點纔會去參與三角化和後續的優化，保證了所有的特徵點質量都是比較高的，同時降低了計算量。

2.2.圖像顯示

	cv::Mat show_img;
	cv::cvtColor(img, show_img, CV_GRAY2RGB);
	if (SHOW_TRACK)
	{
		for (unsigned int j = 0; j < trackerData[0].cur_pts.size(); j++)
        {
            // 點爲按照跟蹤次數的排序，WINDOW_SIZE=10，（255 0 0）紅色代表跟蹤次數少於十次的（大多數），
            //（0 0 255）藍色代表跟蹤次數大於十次的點
			double len = min(1.0, 1.0 * trackerData[0].track_cnt[j] / WINDOW_SIZE);
			cv::circle(show_img, trackerData[0].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
		}
        cv::namedWindow("IMAGE", CV_WINDOW_AUTOSIZE);
		cv::imshow("IMAGE", show_img);
        cv::waitKey(1);//現實圖像窗口
	}

（255 0 0）紅色點代表跟蹤次數少於十次的（大多數），（0 0 255）藍色代表跟蹤次數大於十次的點，顯然一般處於圖像邊緣部分。

3.數據結構

Frame 1: goodFeaturesToTrack 檢測 MAX_CNT 個特徵點,設置 forw_pts 如下:

第一幀的特徵點：

ids	forw_pts	track_cnt
4	[600,400]	1
3	[500,300]	1
2	[400,200]	1
1	[300,100]	1
0	[100,50]	1

ids爲根據跟蹤次數排序後的序號；對應的點和上一幀一一匹配，forw_pts當前幀特徵點座標；track_cnt統計該點被跟蹤的次數。

第二幀的特徵點：

Frame 2: calcOpticalFlowPyrLK 跟蹤,將跟蹤失敗的點刪除,跟蹤成功的點track_cnt跟蹤計數+1,並調用 goodFeaturesToTrack 檢測出 MAX_CNT - forw_pts.size()個特徵點補全每幀的特徵點個數,新檢測的點ids賦值爲-1，同時添加到forw_pts 中,並調用 updateID 更新 ids排序,最後得到的 forw_pts 如下:

ids	forw_pts	track_cnt
6	[200,150]	1
5	[100,100]	1
4	[580,400]	2
1	[280,100]	2
0	[700,50]	2

.　　其中，ids爲0、1、4的爲成功跟蹤的特徵點，track_cnt計數相應+1，ids爲2、3的特徵點跟丟（將status爲0的點，即使跟蹤失敗的點刪除）；ids爲5、6的特徵點爲新檢測加入的點track_cnt計數相應置1.代碼 FeatureTracker::undistortedPoints()中 cur_un_pts 爲歸一化相機座標系下的座標,pts_velocity 爲當前幀相對前一幀特徵點沿 x,y 方向的像素移動速度。

4.接入後端的數據結構

4.1.IMU

imu_msg->header = dStampSec;　　//時間戳
imu_msg->linear_acceleration = vAcc;
imu_msg->angular_velocity = vGyr;
~ ~ ~
imu_buf.push(imu_msg);  //IMU數據入棧！

4.2.Image

feature_points->header = dStampSec;　　//時間戳
~ ~ ~ 
if (trackerData[i].track_cnt[j] > 1)//有匹配的點才入棧!!!!
{
	~ ~ ~
	feature_points->points.push_back(Vector3d(x, y, z));
	feature_points->id_of_point.push_back(p_id * NUM_OF_CAM + i);
	feature_points->u_of_point.push_back(cur_pts[j].x);
	feature_points->v_of_point.push_back(cur_pts[j].y);
	feature_points->velocity_x_of_point.push_back(pts_velocity[j].x);
	feature_points->velocity_y_of_point.push_back(pts_velocity[j].y);
}
~ ~ ~
feature_buf.push(feature_points);  //Image數據處理後入棧！

其他數據結構參考.

5.疑問

進入後端後初步做一個IMU和Image對齊：getMeasurements()函數，使td個IMU數據與一個Image數據對齊，td的值是多少不是固定的，具體在哪兒實例化的沒查到。

 ImgConstPtr img_msg = feature_buf.front();
 feature_buf.pop();
 vector<ImuConstPtr> IMUs;
 while (imu_buf.front()->header < img_msg->header + estimator.td)  //td取值的策略？？？？
   {
       IMUs.emplace_back(imu_buf.front());
       imu_buf.pop();
   }

真正封裝至後端的數據結構：vector<pair<vector<ImuConstPtr>, ImgConstPtr>> measurements;。

VIN1.視覺前端