FeatureTracker的主要作用是跟踪特征并三角化恢复深度以后发给后端去优化。
FeatureTracker功能相关的代码不只是存放在一个文件中，它存放在VINS-Mono/feature_tracker/src文件夹下的所有文件中，这包括：
- feature_tracker_node.cpp：特征跟踪功能的可执行文件，也就是main函数所在的文件
- feature_tracker.cpp：特征跟踪功能的具体实现
- feature_tracker.hpp：特征跟踪功能的头文件
- parameters.cpp：读取参数功能的具体实现
- parameters.hpp：读取参数功能的头文件
话不多说，直接上图，一图胜千言
其实这张图讲的挺详细的了，有了它，也没必要再做太多解释，为了撑场面，我们还是要把重要函数的代码列出来，解释解释。
1. main：入口函数
它位于feature_tracker_node.cpp文件中，主要功能是执行初始化，并确定这个节点文件订阅和发布了哪些数据
主要流程包括：
1）ros初始化和设置句柄，设置logger级别
2）读取如config->euroc->euroc_config.yaml中的一些配置参数
3）读取每个相机实例读取对应的相机内参，NUM_OF_CAM=1为单目
4）判断是否加入鱼眼mask来去除边缘噪声
5）声明订阅和发布的数据
它订阅和发布的数据包括：
输入：
1）订阅topic：“/cam0/image_raw”：
即传感器或者rosbag发布的图像
输出：
1）发布topic：“/feature_trackers/feature_img”：
即跟踪的特征点图像，主要是之后给RVIZ用和调试用
2）发布topic：“/feature_trackers/feature”
即跟踪的特征点信息，由/vins_estimator订阅并进行优化
3）发布topic：“/feature_trackers/restart”
即判断特征跟踪模块是否出错，若有问题则进行复位，由/vins_estimator订阅
2. img_callback：接收图像的回调函数
它位于feature_tracker_node.cpp文件中，是特征跟踪功能的入口，接收到图像以后做一些基本的处理，然后调用feature_tracker.cpp文件中关于特征跟踪功能的具体实现函数。
主要流程包括：
1）判断是否是第一帧
2）判断时间间隔是否正确，有问题则restart
3）发布频率控制，并不是每读入一帧图像，就要发布特征点，通过判断间隔时间内的发布次数（不发布的时候也是会执行readImage() 读取图像进行处理）
4）将图像编码8UC1转换为mono8
5）单目时：FeatureTracker::readImage() 函数读取图像数据进行处理
6）更新全局ID
7）如果PUB_THIS_FRAME=1则进行发布
3. readImage：提取并跟踪图像特征
readImage(
const cv::Mat &_img, //图像
double _cur_time)//时间戳
它位于feature_tracker.cpp文件中，是整个特征跟踪功能的全部，其他函数主要是为了帮助他实现功能，是它的子函数
它的主要流程包括：
1）createCLAHE() 判断并对图像进行自适应直方图均衡化；
2）calcOpticalFlowPyrLK() 从cur_pts到forw_pts做LK金字塔光流法；
3）根据status，把跟踪失败的和位于图像边界外的点剔除，剔除时不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts、prev_pts、cur_pts，记录特征点id的ids，和记录特征点被跟踪次数的track_cnt中剔除；
4）setMask() 对跟踪点进行排序并依次选点，设置mask：去掉密集点，使特征点分布均匀
5）rejectWithF() 通过基本矩阵F剔除outliers
6）goodFeaturesToTrack() 寻找新的特征点(shi-tomasi角点)，添加(MAX_CNT - forw_pts.size())个点以确保每帧都有足够的特征点
7）addPoints()向forw_pts添加新的追踪点，id初始化-1，track_cnt初始化为1
8）undistortedPoints() 对特征点的图像坐标根据不同的相机模型进行去畸变矫正和深度归一化，计算每个角点的速度
下面直接上代码
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
cv::Mat img;
TicToc t_r;
cur_time = _cur_time;
//如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理
if (EQUALIZE)
{
//自适应直方图均衡
//createCLAHE(double clipLimit, Size tileGridSize)
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
TicToc t_c;
clahe->apply(_img, img);
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
else
img = _img;
if (forw_img.empty())
{
//如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据
//将读入的图像赋给当前帧forw_img，同时还赋给prev_img、cur_img
prev_img = cur_img = forw_img = img;
}
else
{
//否则，说明之前就已经有图像读入，只需要更新当前帧forw_img的数据
forw_img = img;
}
//此时forw_pts还保存的是上一帧图像中的特征点，所以把它清除
forw_pts.clear();
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
//调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪，得到forw_pts
//status标记了从前一帧cur_img到forw_img特征点的跟踪状态，无法被追踪到的点标记为0
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
//将位于图像边界外的点标记为0
for (int i = 0; i < int(forw_pts.size()); i++)
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
//根据status,把跟踪失败的点剔除
//不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts、prev_pts和cur_pts中剔除
//prev_pts和cur_pts中的特征点是一一对应的
//记录特征点id的ids，和记录特征点被跟踪次数的track_cnt也要剔除
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(cur_un_pts, status);
reduceVector(track_cnt, status);
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}
//光流追踪成功,特征点被成功跟踪的次数就加1
//数值代表被追踪的次数，数值越大，说明被追踪的就越久
for (auto &n : track_cnt)
n++;
//PUB_THIS_FRAME=1 需要发布特征点
if (PUB_THIS_FRAME)
{
//通过基本矩阵剔除outliers
rejectWithF();
ROS_DEBUG("set mask begins");
TicToc t_m;
setMask();//保证相邻的特征点之间要相隔30个像素,设置mask
ROS_DEBUG("set mask costs %fms", t_m.toc());
ROS_DEBUG("detect feature begins");
TicToc t_t;
//计算是否需要提取新的特征点
int n_max_cnt = MAX_CNT - static_cast<int>(forw_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;
if (mask.size() != forw_img.size())
cout << "wrong size " << endl;
/**
*void cv::goodFeaturesToTrack( 在mask中不为0的区域检测新的特征点
* InputArray image, 输入图像
* OutputArray corners, 存放检测到的角点的vector
* int maxCorners, 返回的角点的数量的最大值
* double qualityLevel, 角点质量水平的最低阈值（范围为0到1，质量最高角点的水平为1），小于该阈值的角点被拒绝
* double minDistance, 返回角点之间欧式距离的最小值
* InputArray mask = noArray(), 和输入图像具有相同大小，类型必须为CV_8UC1,用来描述图像中感兴趣的区域，只在感兴趣区域中检测角点
* int blockSize = 3, 计算协方差矩阵时的窗口大小
* bool useHarrisDetector = false, 指示是否使用Harris角点检测，如不指定则使用shi-tomasi算法
* double k = 0.04 Harris角点检测需要的k值
*)
*/
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}
else
n_pts.clear();
ROS_DEBUG("detect feature costs: %fms", t_t.toc());
ROS_DEBUG("add feature begins");
TicToc t_a;
//添将新检测到的特征点n_pts添加到forw_pts中，id初始化-1,track_cnt初始化为1.
addPoints();
ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
}
//当下一帧图像到来时，当前帧数据就成为了上一帧发布的数据
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
//把当前帧的数据forw_img、forw_pts赋给上一帧cur_img、cur_pts
cur_img = forw_img;
cur_pts = forw_pts;
//根据不同的相机模型去畸变矫正和转换到归一化坐标系上，计算速度
undistortedPoints();
prev_time = cur_time;
}
4. setMask：对跟踪点进行排序并去除密集点
为了是特征点分布更均匀，对跟踪到的特征点，按照被追踪到的次数排序并依次选点，使用mask进行类似非极大抑制，半径为30，去掉密集点。
void FeatureTracker::setMask()
{
if(FISHEYE)
mask = fisheye_mask.clone();
else
mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
// prefer to keep features that are tracked for long time
// 构造(cnt，pts，id)序列
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;
for (unsigned int i = 0; i < forw_pts.size(); i++)
cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));
//对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序（lambda表达式）
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;
});
//清空cnt，pts，id并重新存入
forw_pts.clear();
ids.clear();
track_cnt.clear();
for (auto &it : cnt_pts_id)
{
if (mask.at<uchar>(it.second.first) == 255)
{
//当前特征点位置对应的mask值为255，则保留当前特征点，将对应的特征点位置pts，id，被追踪次数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);
}
}
}
5. rejectWithF：通过基础矩阵F去除outliers
首先将将图像坐标畸变矫正后转换为深度归一化坐标，通过cv::findFundamentalMat()计算F矩阵，利用得到的status通过reduceVector()去除outliers。
void FeatureTracker::rejectWithF()
{
if (forw_pts.size() >= 8)
{
ROS_DEBUG("FM ransac begins");
TicToc t_f;
vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
Eigen::Vector3d tmp_p;
//根据不同的相机模型将二维坐标转换到三维坐标
m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
//转换为归一化像素坐标
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
}
vector<uchar> status;
//调用cv::findFundamentalMat对un_cur_pts和un_forw_pts计算F矩阵
cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
int size_a = cur_pts.size();
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(cur_un_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
}
}
评论（0）
您还未登录，请登录后发表或查看评论