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;。