VINS_FUSION入门系列－特征点的跟踪
1.1 跟踪的具体函数
// 对图片进行一系列操作，返回特征点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)
{
if(mask.empty())
cout << "mask is empty " << endl;
if (mask.type() != CV_8UC1)
cout << "mask type wrong " << endl;
/* goodFeaturesToTrack
_image：8位或32位浮点型输入图像，单通道
_corners：保存检测出的角点
maxCorners：角点数目最大值，如果实际检测的角点超过此值，则只返回前maxCorners个强角点
qualityLevel：角点的品质因子
minDistance：对于初选出的角点而言，如果在其周围minDistance范围内存在其他更强角点，则将此角点删除
_mask：指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI
blockSize：计算协方差矩阵时的窗口大小
useHarrisDetector：指示是否使用Harris角点检测，如不指定，则计算shi-tomasi角点
harrisK：Harris角点检测需要的k值 */
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
// mask 这里肯定是指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI
}
else
n_pts.clear();
对cv::goodFeaturesToTrack(）函数的理解
一提到角点检测，最常用的方法莫过于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)
{
TicToc t_o;
vector status;
vector err;
if(hasPrediction)
{
cur_pts = predict_pts; //当前的点等于上一次的点
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1);
//迭代算法的终止条件
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
int succ_num = 0;//成功和上一阵匹配的数目
for (size_t i = 0; i < status.size(); i++)
{
if (status[i])
succ_num++;
}
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)//
{
vector reverse_status;
vector reverse_pts = prev_pts;
//注意！这里输入的参数和上边的前后是相反的
cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
//cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 3);
for(size_t i = 0; i < status.size(); i++)
{
//如果前后都能找到，并且找到的点的距离小于0.5
if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
{
status[i] = 1;
}
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)
{
//int rows = imLeft.rows;
int cols = imLeft.cols;
// ------------将两幅图像进行拼接
if (!imRight.empty() && stereo_cam)
cv::hconcat(imLeft, imRight, imTrack);
// 图像凭借hconcat（B,C，A）; // 等同于A=[B C]
else
imTrack = imLeft.clone();
cv::cvtColor(imTrack, imTrack, CV_GRAY2RGB);
//将imTrack转换为彩色
// -------------在左目图像上标记特征点
for (size_t j = 0; j < curLeftPts.size(); j++)
{
double len = std::min(1.0, 1.0 * track_cnt[j] / 20);//FIXME: 这个是画圈的颜色问题
cv::circle(imTrack, curLeftPts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
}
// -------------在右目图像上标记特征点
if (!imRight.empty() && stereo_cam)
{
for (size_t i = 0; i < curRightPts.size(); i++)
{
cv::Point2f rightPt = curRightPts[i];
rightPt.x += cols;//计算在凭借的图像上，右目特征点的位置
cv::circle(imTrack, rightPt, 2, cv::Scalar(0, 255, 0), 2);
//画出左右目的匹配直线 curLeftPtsTrackRight找不到啊！！
// cv::Point2f leftPt = curLeftPtsTrackRight[i];
// cv::line(imTrack, leftPt, rightPt, cv::Scalar(0, 255, 0), 1, 8, 0);
}
}
map::iterator mapIt;
for (size_t i = 0; i < curLeftIds.size(); i++)
{
int id = curLeftIds[i];
mapIt = prevLeftPtsMap.find(id);
if(mapIt != prevLeftPtsMap.end())
{
cv::arrowedLine(imTrack, curLeftPts[i], mapIt->second, cv::Scalar(0, 255, 0), 1, 8, 0, 0.2);
// 在imTrack上，从curLeftPts到mapIt->second画箭头
}
}
}
当先前已经有跟踪点了，然后需要在提取新的特征点，在没有提取特征点的地方进行提取，先前提取了特征的地方mask为０．
//TODO: 这里应该可以做动态检测
// 把追踪到的点进行标记
// 设置遮挡部分（鱼眼相机）
// 对检测到的特征点按追踪到的次数排序
// 在mask图像中将追踪到点的地方设置为0，否则为255，目的是为了下面做特征点检测的时候可以选择没有特征点的区域进行检测。
// 在同一区域内，追踪到次数最多的点会被保留，其他的点会被删除
void FeatureTracker::setMask()
{
mask = cv::Mat(row, col, CV_8UC1, cv::Scalar(255)); // 标记点的图像
// 保存长时间跟踪到的特征点 prefer to keep features that are tracked for long time
vector>> cnt_pts_id;// 帧id，点位置和点id
for (unsigned int i = 0; i < cur_pts.size(); i++)
cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(cur_pts[i], ids[i])));//把追踪得到的点track_cnt放入cnt_pts_id
// sort 对给定区间的所有元素进行排序，按照点的跟踪次数，从多到少进行排序
sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair> &a, const pair> &b)
{
return a.first > b.first;
});
cur_pts.clear();
ids.clear();
track_cnt.clear();
for (auto &it : cnt_pts_id)//对于所有追踪到的点
{
//将跟踪到的点按照跟踪次数重新排列，并返回到forw_pts，ids，track_cnt
if (mask.at(it.second.first) == 255)//如果在特征点出的灰度值等于255
// at 获取像素点的灰度值或者RGB值，可以通过image.at(i,j)的方式轻松获取。检测新建的mask在该点是否为255
{
cur_pts.push_back(it.second.first);
ids.push_back(it.second.second);
track_cnt.push_back(it.first);
/*在已跟踪到角点的位置上，将mask对应位置上设为0,
意为在cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
进行操作时在该点不再重复进行角点检测，这样可以使角点分布更加均匀
TODO:讲动态的特征点,也设置为这个
图片，点，半径，颜色为0表示在角点检测在该点不起作用,粗细（-1）表示填充*/
cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
}
// TODO:这里应该输出一下mask 看看效果
// cv::imshow ( "mask", mask ); // 用cv::imshow显示图像
// cv::waitKey ( 0 ); // 暂停程序,等待一个按键输入
}
}
当如果是双目的时候对左目右目的点进行跟踪，找到双目的匹配点，为后续的三角化和pnp求解位置做准备。
// ---------------如果是双目的
if(!_img1.empty() && stereo_cam)
// 把左目的点在右目上找到，然后计算右目上的像素速度。
{
ids_right.clear();
cur_right_pts.clear();
cur_un_right_pts.clear();
right_pts_velocity.clear();
cur_un_right_pts_map.clear();
// --------------------如果当前帧非空
if(!cur_pts.empty())
{
//printf("stereo image; track feature on right image\n"); //在右侧图像上追踪特征
vector reverseLeftPts;
vector status, statusRightLeft; //左右目的状态
vector err;
/*光流跟踪是在左右两幅图像之间进行cur left ---- cur right
prevImg 第一幅8位输入图像 或 由buildOpticalFlowPyramid()构造的金字塔。
nextImg 第二幅与preImg大小和类型相同的输入图像或金字塔。
prevPts 光流法需要找到的二维点的vector。点坐标必须是单精度浮点数。
nextPts 可以作为输入，也可以作为输出。包含输入特征在第二幅图像中计算出的新位置的二维点（单精度浮点坐标）的输出vector。当使用OPTFLOW_USE_INITIAL_FLOW 标志时，nextPts的vector必须与input的大小相同。
status 输出状态vector(类型：unsigned chars)。如果找到了对应特征的流，则将向量的每个元素设置为1；否则，置0。
err 误差输出vector。vector的每个元素被设置为对应特征的误差，可以在flags参数中设置误差度量的类型；如果没有找到流，则未定义误差（使用status参数来查找此类情况）。
winSize 每级金字塔的搜索窗口大小。
maxLevel 基于最大金字塔层次数。如果设置为0，则不使用金字塔（单级）；如果设置为1，则使用两个级别，等等。如果金字塔被传递到input，那么算法使用的级别与金字塔同级别但不大于MaxLevel。
criteria 指定迭代搜索算法的终止准则（在指定的最大迭代次数标准值（criteria.maxCount）之后，或者当搜索窗口移动小于criteria.epsilon。）
flags 操作标志，可选参数：
OPTFLOW_USE_INITIAL_FLOW：使用初始估计，存储在nextPts中；如果未设置标志，则将prevPts复制到nextPts并被视为初始估计。
OPTFLOW_LK_GET_MIN_EIGENVALS：使用最小本征值作为误差度量（见minEigThreshold描述）；如果未设置标志，则将原始周围的一小部分和移动的点之间的 L1 距离除以窗口中的像素数，作为误差度量。
minEigThreshold
算法所计算的光流方程的2x2标准矩阵的最小本征值（该矩阵称为[Bouguet00]中的空间梯度矩阵）÷ 窗口中的像素数。如果该值小于MinEigThreshold，则过滤掉相应的特征，相应的流也不进行处理。因此可以移除不好的点并提升性能。 */
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
// 反向跟踪reverse check cur right ---- cur left
if(FLOW_BACK)
{
cv::calcOpticalFlowPyrLK(rightImg, cur_img, cur_right_pts, reverseLeftPts, statusRightLeft, err, cv::Size(21, 21), 3);
for(size_t i = 0; i < status.size(); i++)
{
if(status[i] && statusRightLeft[i] && inBorder(cur_right_pts[i]) && distance(cur_pts[i], reverseLeftPts[i]) <= 0.5)
status[i] = 1;
else
status[i] = 0;
}
}
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;
}
２．小结
这部分主要是为了利用ＬＫ光流找出相邻两帧的特征点的匹配关系，为后续的求最小重投影误差做准备，利用ＬＫ光流找出左右目之间的匹配关系，为后续的三角化做准备，总体来说这个过程相对与描述子的匹配耗时是少很多的。