FeatureTracker::trackImage()
包含了:帧间光流法、区域mask、检测特征点、左右目光流法匹配,计算像素速度,画图.
跟踪上一帧的特征点
如果已经有特征点,就直接进行LK追踪,新的特征点存到cur_pts
中(如果刚开始还没有特征点,就直接用cv::goodFeaturesToTrack
检测特征点):
(现根据上次速度和prev_pts
,预测本次predict_pts
,然后cur_pts = predict_pts
这样能缩短光流法时间)
cur_pts = predict_pts;
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1);
如果跟踪到的特征点过少(状态位之和<阈值),换个尺度继续找
if (succ_num < 10)
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
然后反向使用光流法提高准确性,用预测值cur_pts去反向预测输入prev_pts,看看是不是对的上,只有两次光流法对应上了(距离小于某值),才让状态位status=1。
cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1)
if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
{
status[i] = 1;
}
特征点的信息:特征点ids、特征点坐标cur_pts、跟踪到的次数track_cnt。 如果那个特征点status为0,就用reduceVector
剔除这些信息。
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
对跟踪到的点,跟踪次数+1。
检测新的特征点
接下来利用FeatureTracker::setMask()
来设置mask,规则是:
1.将特征点按在以往帧中出现的次数降序排列 (track_cnt
存储了次数)
2.从最多的开始,按某一半径设置mask并删除范围内其他点,保证特征点的均匀分布。
然后在mask以外的区域检测新的特征点:
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
// goodFeaturesToTrack(图像,输出点,最多个数,品质因子,角点间距,检测范围)
然后将新提取出来的特征点,添加它的信息:特征点ids、特征点坐标cur_pts、跟踪到的次数track_cnt。
用FeatureTracker::undistortedPts()
函数,将当前特征点cur_pts
的像素坐标(u, v),转化到归一化相机平面(x,y,1),放到cur_un_pts
。
用FeatureTracker::ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map)
求当前帧相对于前一帧,特征点沿x,y方向的像素移动速度=(当前点-先前点)/时间,放到pts_velocity
.
从左目特征点检测右目对应点
利用光流法做左右目匹配,方法和左目的前后帧匹配一样,匹配到的点,ids相同放到ids_right,坐标放到cur_right_pts:
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
// 对应于 cur_pts 寻找在 cur_right_pts 中的对应点
然后同上,转化到归一化相机平面,存到cur_un_right_pts
,计算像素速度,存到right_pts_velocity
左右目图像拼接,并画出左目、右目特征点和在不同t的帧间变化连线:
drawTrack(cur_img, rightImg, ids, cur_pts, cur_right_pts, prevLeftPtsMap);
构建map:featureFrame
构建map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame
,填入{特征点id, [相机id, (特征点空间坐标,像素坐标,像素速度)]}。
feature_id = ids[i]
x = cur_un_pts[i].x;
y = cur_un_pts[i].y;
z = 1;
p_u = cur_pts[i].x;
p_v = cur_pts[i].y;
velocity_x = pts_velocity[i].x;
velocity_y = pts_velocity[i].y;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
填入featureBuf
featureBuf.push(make_pair(t, featureFrame));
也就是,每帧一个featureFrame,存储该帧检测到的特征点的所有信息。
补充:前面的ids、cur_pts、track_cnt的一点解释。
如第一帧检测到了5个特征点,那它的特征点ids、特征点坐标cur_pts、跟踪到的次数track_cnt是这样的:
第二帧跟踪到了第0、1、4个,然后新检测到了2个点,第二帧的这些量就是这样:
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)