VINS 代码学习（一）--feature_tracker 代码
声明：
1）该VINS 代码的学习系列主要是为了记录自己学习VINS代码的过程。
2）若有错误，欢迎各位大牛吐槽，如有版权问题请联系：[email protected]
**
网络上已经有论文的解析，本文只给出以下两个特征提取部分的代码的注释。
- featrue_trakcer_node.cpp
- feature_tracker.cpp
feature_tracker_node.cpp
#include clahe = cv::createCLAHE();// 直方图均衡化
clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
}
else
trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
}
#if SHOW_UNDISTORTION
trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
#endif
}
if ( PUB_THIS_FRAME && STEREO_TRACK && trackerData[0].cur_pts.size() > 0)
{
pub_count++;
r_status.clear();
r_err.clear();
TicToc t_o;
// 双目模式下，当要发部特征，则左右两帧图像做光流，（有一种结合了直接法，光流法去除outliers）
cv::calcOpticalFlowPyrLK(trackerData[0].cur_img, trackerData[1].cur_img, trackerData[0].cur_pts, trackerData[1].cur_pts, r_status, r_err, cv::Size(21, 21), 3);
ROS_DEBUG("spatial optical flow costs: %fms", t_o.toc());
vector("feature", 1000); //发布了点云
pub_match = n.advertise("feature_img",1000); //发布特征图片，后面有待考察
/*
if (SHOW_TRACK)
cv::namedWindow("vis", cv::WINDOW_NORMAL);
*/
ros::spin();
return 0;
}
feature_trakcer.cpp
#include "feature_tracker.h"
int FeatureTracker::n_id = 0;
bool inBorder(const cv::Point2f &pt) // 判断点是否在边界内
{
const int BORDER_SIZE = 1;
int img_x = cvRound(pt.x);
int img_y = cvRound(pt.y);
return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;
}
void reduceVector(vectorint , pairint>>> 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 中的点排序 按照 track_cnt[i] 由大到小排序。sort(begin, end, compare)
sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pairint>> &a, const pair<int, pairint>> &b)
{
return a.first > b.first;
});
forw_pts.clear();
ids.clear();
track_cnt.clear();
for (auto &it : cnt_pts_id)
{
if (mask.at(it.second.first) == 255) // at 为取出图像中的点。当该点的灰度值为255，
{
forw_pts.push_back(it.second.first); // 将满足上述条件的点push到forw_pts 中
ids.push_back(it.second.second); // 保存对应的id
track_cnt.push_back(it.first); // 保存对应对应forw_pts 在 cnt_pts_id 里面的索引
cv::circle(mask, it.second.first, MIN_DIST, 0, -1); // mask 一定范围内 设为 ０
}
}
}
void FeatureTracker::addPoints()
{
for (auto &p : n_pts)
{
forw_pts.push_back(p);
ids.push_back(-1);
track_cnt.push_back(1);
}
}
void FeatureTracker::readImage(const cv::Mat &_img)
{
cv::Mat img;
TicToc t_r;
if (EQUALIZE) // 判断均衡化的标志
{
cv::Ptr 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())
{
prev_img = cur_img = forw_img = img;
}
else
{
forw_img = img;
}
forw_pts.clear();
if (cur_pts.size() > 0) //在已提取了上一帧特征点的情况下进入
{
TicToc t_o;
vector(1, 0), p.at(0, 0));
//printf("%lf %lf\n", pp.at(1, 0), pp.at(0, 0));
if (pp.at<float>(1, 0) + 300 >= 0 && pp.at<float>(1, 0) + 300 < ROW + 600 && pp.at<float>(0, 0) + 300 >= 0 && pp.at<float>(0, 0) + 300 < COL + 600)
{
undistortedImg.at(pp.at<float>(1, 0) + 300, pp.at<float>(0, 0) + 300) = cur_img.at(distortedp[i].y(), distortedp[i].x());
}
else
{
//ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, pp.at(1, 0), pp.at(0, 0));
}
}
cv::imshow(name, undistortedImg);
cv::waitKey(0);
}
vector