Vins-Mono 阅读笔记——前端
1、前端流程概述
VINS-Mono的前端整个封装成了一个ROS节点
其订阅的topic是：
- 相机或者数据集发来的图片
其发布topic是：
- 由pub_img发布的"feature"，发布的是当前帧的特征点，特征点分装成了sensor_msgs::PointCloudPtr类型，里面包括了每个特征点的（1）归一化平面上的坐标（2）归一化平面上的速度（3）图像平面上的坐标，这三者中归一化平面坐标是在SFM和后端用到，而归一化平面上的速度以及图像平面上的坐标仅仅在进行时间偏差估计时会用到。
- 由pub_match发布的“feature_img”，发布的是用来调试的带特征点的灰度图，图上特征点的颜色越红代表此特征点被跟踪的帧数越多，越蓝表示越少。
在图片的回调函数中会先对时间进行控制，然后会进入 readImage() 函数 进行光流追踪、特征点提取、特征点提取等步骤
2、feature_tracker
feature_tracker是vins的前端，它的目录在src/ feature_tracker下，功能主要是获取摄像头的图像帧，并按照事先设定的频率，把cur帧上满足要求的特征点以sensor_msg::PointCloudPtr的格式发布出去，以便RVIZ和vins——estimator接收。
这个package下面主要包括4个功能：
feature_tracker——包含特征提取/光流追踪的所有算法函数；
feature_tracker_node——特征提取的主入口，负责一个特征处理结点的功能；
parameters——负责读取来自配置文件的参数；
tic_tok——计时器；
2.1 、feature_tracker_node
在开头定义了一些变量，遇到的时候再一一解释。
数据结构: FeatureTracker trackerData[NUM_OF_CAM]
定义在feature_tracker.h中的一个class，除了一系列成员函数外(注意，inBorder(),reduceVector()并没有定义在这个class里)，还包括了最近3帧所有特征点的一些信息。如何理解这个变量？它存储着当前时刻所有相关的数据，内部数据的生命周期仅限当前循环，如果当前的一个循环过去了，里面的数据是下一时刻的了。
它包括以下数据成员，
cv::Mat mask;//图像掩码
cv::Mat fisheye_mask;//鱼眼相机mask，用来去除边缘噪点
cv::Mat prev_img, cur_img, forw_img;//是上上次发布的帧的图像数据/光流跟踪的上一帧的图像数据/当前的图像数据
vectorcv::Point2f n_pts;//每一帧中新提取的特征点
vectorcv::Point2f prev_pts, cur_pts, forw_pts;//对应的图像特征点
vectorcv::Point2f prev_un_pts, cur_un_pts;//归一化相机坐标系下的坐标
vectorcv::Point2f pts_velocity;//当前帧相对前一帧特征点沿x,y方向的像素移动速
vectorids;//能够被跟踪到的特征点的id
vectortrack_cnt;//当前帧forw_img中每个特征点被追踪的时间次数
map<int, cv::Point2f> cur_un_pts_map; //构建id与归一化坐标的id，见undistortedPoints()
map<int, cv::Point2f> prev_un_pts_map;
camodocal::CameraPtr m_camera;//相机模型
double cur_time;
double prev_time;
static int n_id;//用来作为特征点id，每检测到一个新的特征点，就将++n_id作为该特征点
工作流程：
1、ros初始化和设置句柄，设置logger级别；
2、读取如config->euroc->euroc_config.yaml中的一些配置参数；
3、读取每个相机实例对应的相机内参，NUM_OF_CAM=1为单目；
4、判断是否加入鱼眼mask来去除边缘噪声
5、订阅话题IMAGE_TOPIC，执行回调函数img_callback
6、发布
int main(int argc, char **argv)
{
ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
readParameters(n);
for (int i = 0; i < NUM_OF_CAM; i++)
trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);
if(FISHEYE)
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
if(!trackerData[i].fisheye_mask.data)
{
ROS_INFO("load mask fail");
ROS_BREAK();
}
else
ROS_INFO("load mask success");
}
}
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
/*
if (SHOW_TRACK)
cv::namedWindow("vis", cv::WINDOW_NORMAL);
*/
ros::spin();
return 0;
}
2.2 img_callback
该函数是ROS的回调函数，主要功能包括：readImage()函数对新来的图像使用光流法进行特征点跟踪，并将追踪的特征点封装成feature_points发布到pub_img的话题下，将图像封装成ptr发布在pub_match下。
2.2.1 对第一帧图像的处理
//对第一帧图像进行处理
if(first_image_flag)
{
first_image_flag = false;
first_image_time = img_msg->header.stamp.toSec();
last_image_time = img_msg->header.stamp.toSec();
return;
}
对于第一帧图像，只记录对应时间戳，不提取特征，因为他没有前一帧图像，无法获取光流。
2.2.2 频率控制
(1)对于时间戳错乱的帧，重新初始化；
if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
{
ROS_WARN("image discontinue! reset the feature tracker!");
first_image_flag = true;
last_image_time = 0;
pub_count = 1;
std_msgs::Bool restart_flag;
restart_flag.data = true;
pub_restart.publish(restart_flag);
return;
}
(2) 更新: last_image_time = img_msg->header.stamp.toSec();
(3)计算当前累计的pub_count个帧的频率与FREQ的关系
- 如果实际发布频率大于设定值，肯定就不发了
所以，要想发布图像帧，那么实际频率要比设定值小
但是，如果实际频率与设定频率的累积误差大于0.01了，就不能发布这一帧
if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
//TODO:这句话可以放到下面那个if里面把？
PUB_THIS_FRAME = true;//发布特征点
// reset the frequency control
if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
{ //this "if" make sure the frequece is not so high
first_image_time = img_msg->header.stamp.toSec();
// TODO:我觉得pub_count最大只能是1，因为只要能发布一帧，这个数就会被清0
pub_count = 0;
}
}
else
PUB_THIS_FRAME = false;
2.2.3 图像格式调整和图像读取
cv_bridge::CvImageConstPtr ptr;
if (img_msg->encoding == "8UC1") //8位灰度图像
{
sensor_msgs::Image img; // I think it is to change 'sensor_msgs::ImageConstPtr'(const) to non-const
img.header = img_msg->header;
img.height = img_msg->height;
img.width = img_msg->width;
img.is_bigendian = img_msg->is_bigendian;
img.step = img_msg->step;
img.data = img_msg->data;
img.encoding = "mono8";
ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);//将图像编码8UC1转换为mono8,即存储下来的图像为单色，8Bit的图片，一般是bmp，jpeg等
}
else
ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
cv::Mat show_img = ptr->image;
2.2.4 对最新帧forw的特征点进行提取和光流跟踪
img_callback()最最核心的语句出现在这里，也就是readImage()，这个函数实现了特征的处理和光流的追踪，里面基本上调用了feature_tracker.cpp里面的全部函数。在这里首先进行了单目和双目的判断，由于VINS-Mono是单目的，所以后部分关于双目的判断不用看，作者写在这里我觉得是为VINS-Fusion做准备的。
for (int i = 0; i < NUM_OF_CAM; i++)
{
ROS_DEBUG("processing camera %d", i);
if (i != 1 || !STEREO_TRACK)//单目时：FeatureTracker::readImage() 函数读取图像数据进行处理
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
readImage()传了2个参数，当前帧的图像和当前帧的时间戳,以下内容见feature_tracker.cpp，看一下原函数里面是怎么写的。
(1) 自适应直方图均衡化
if (EQUALIZE) //判断是否对图像进行自适应直方图均衡化
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//对图像进行自适应直方图均衡化
TicToc t_c;
clahe->apply(_img, img); //apply(InputArray src, OutputArray dst)
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
else
img = _img;
(2)首帧判断和对的forw_img更新
if (forw_img.empty()) //如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据
{ //将读入的图像赋给当前帧forw_img
prev_img = cur_img = forw_img = img;//还将读入的图像赋给prev_img、cur_img
} //这是为了避免后面使用到这些数据时，它们是空的
else
{
forw_img = img;
}
(3) forw_pts.clear()
(4)光流追踪和失败点剔除
在这里实现了3大功能：光流追踪，outlier的剔除，并且不同容器里的数据仍然是对齐的！在《SLAM十四讲》第13讲的代码里，前端的对齐策略是往容器里面加空指针。这里通过reduceVector()实现对齐更为简单，避免了大量的指针操作，实际上，VINS的数据结构更多采用hash的方式，而《SLAM十四讲》更多地采用了指针。
这里通过inBorder(),status()来定位到outlier的位置，然后把status[i]==0的点统统剔除。
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
//! 光流跟踪
//cur_img 前一帧图像
//forw_img 当前帧图像
//cur_pt 要跟踪，前一帧图像的，哪些特征点
//forw_pts 前一帧图像的特征点，对应当前帧的哪些特征点
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
for (int i = 0; i < int(forw_pts.size()); i++)//将位于图像边界外的点标记为0
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
//! 将标记为0的点去掉（用后面的覆盖掉前面的）
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());
}
(5)更新当前特征点被追踪到的次数
//!更新当前特征点被追踪到的次数
for (auto &n : track_cnt)
n++;
(6) 通过基本矩阵剔除外点rejectWithF()
if (PUB_THIS_FRAME)
{
//通过基本矩阵剔除外点
rejectWithF();
TicToc t_m;
setMask();
TicToc t_t;
。。。。
- rejectWithF()主要是利用了cv::findFundamentalMat()这个函数来进一步剔除outlier。
这个函数的功能对应在《SLAM十四讲》第七讲2D-2D对极几何相关知识点，两帧上的一系列对应的特征点能够复原出两帧之间的相对位姿变化，也就是基础矩阵E。但是这些特征点中肯定会有一些outlier，所以通过这个opencv的函数，能够巧妙地剔除这些outlier。补充一点，如果函数传入的是归一化坐标，那么得到的是本质矩阵E，如果传入的是像素坐标，那么得到的是基础矩阵F。
首先看一下rejectWithF()，这一段代码的逻辑，我的理解，是先把特征点坐标(像素坐标)转为归一化坐标，再转回到像素坐标，然后再用findFundamentalMat()找outlier。它之所以这么做，是因为需要一个去畸变的过程，在m_camera->liftProjective()里实现，m_camera是定义在camera.h的一个父类的对象，下面有多个相机模型的子类，例如在PinholeCamera.cc里，这里面就重写liftProjective()了方法，实现了去畸变的功能。
(7) 对跟踪点进行排序并去除密集点setMask()
注意，这个函数的操作对象是forw_pts！这里，需要再次明确一下forw_pts里面是什么，它是通过光流法在当前帧中仍然能追踪到的那些特征点在当前帧中的像素坐标！所以forw_pts里面放着的都是老特征点。这里干了2件事：
- 首先对他们根据追踪到的次数进行排序，从而确定了优先级；
- 然后再把特征点周围的密集的特征点剔除掉。
所以说，排序的作用就是为了给 剔除操作 提供优先级信息！因为追踪次数多的点在前面，所以在剔除的时候，这些时间长的特征点能够保留下来，而在这些老特征点周围比较近的距离内那些比较新的特征点就剔除掉，从而一方面保证了特征点的稳定，另一方面让特征点的分布更加均匀。
void FeatureTracker::setMask()
{
if(FISHEYE)
mask = fisheye_mask.clone();
else
mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));//将图像设置成单一灰度和颜色white
// prefer to keep features that are tracked for long time
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;//构造(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])));
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)
{//对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序
return a.first > b.first;
});
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);
cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
}//在mask中将当前特征点周围半径为MIN_DIST的区域设置为0，后面不再选取该区域内的点（使跟踪点不集中在一个区域上）
}
}
其大概流程是会根据光流法跟踪下一帧图片forw_img获得的forw_pts的被跟踪的时间进行排序，然后按照被跟踪时间从长到段的顺序以半径MIN_DIST在一张白图上画黑圈，如果画出来的mask大概长下面这个样子：
将这个mask图输入到goodFeaturesToTrack函数中，函数就只会在白色区域，也就是没有跟踪时长特别长的地区提取特征点，因此会使得我们提取的关键点相对较为均匀，如下图所示：
如果将setMask函数中的排序部分去掉，特征点分布变成如下效果
** (8)提取新特征点goodFeaturesToTrack()**
一共需要MAX_CNT个特征点，当前有static_cast(forw_pts.size())个特征点，所以需要补充n_max_cnt = MAX_CNT - static_cast(forw_pts.size())个特征点。
....
//MAX_CNT 一共需要的特征点
//static_cast<int>(forw_pts.size()) 当前已经有了多少特征点
//n_max_cnt 还需要补充多少个特征点
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;
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}
(9) 补充新特征点addPoints()
void FeatureTracker::addPoints()
{
for (auto &p : n_pts)
{ forw_pts.push_back(p); ids.push_back(-1); track_cnt.push_back(1); }
}
(10)数据的更新和归一化坐标的获取
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
cur_img = forw_img;
cur_pts = forw_pts;
undistortedPoints();
prev_time = cur_time;
undistortedPoints();这个函数干了2件事：
- 第一个是获取forw时刻去畸变的归一化坐标(这个是要发布到rosmsg里的points数据)，
- 另一个是获取forw时刻像素运动速度。
void FeatureTracker::undistortedPoints()
{// 对特征点的图像坐标根据不同的相机模型进行去畸变矫正和深度归一化，计算每个角点的速度
cur_un_pts.clear();
cur_un_pts_map.clear();
//cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
Eigen::Vector3d b;
m_camera->liftProjective(a, b);
//获取cur帧下的归一化坐标和它与id的hash(这个hash唯一的用途也在这个函数里)
cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));
//printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
}
// caculate points velocity
if (!prev_un_pts_map.empty())
{
double dt = cur_time - prev_time;
pts_velocity.clear();
for (unsigned int i = 0; i < cur_un_pts.size(); i++)
{
if (ids[i] != -1)
{
std::map<int, cv::Point2f>::iterator it;
it = prev_un_pts_map.find(ids[i]);
if (it != prev_un_pts_map.end())
{
double v_x = (cur_un_pts[i].x - it->second.x) / dt;
double v_y = (cur_un_pts[i].y - it->second.y) / dt;
pts_velocity.push_back(cv::Point2f(v_x, v_y));
}
else //TODO 这个判断可以删掉吧
pts_velocity.push_back(cv::Point2f(0, 0));
}
else
{pts_velocity.push_back(cv::Point2f(0, 0));}
}
}
else
{
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
pts_velocity.push_back(cv::Point2f(0, 0));
}
}
prev_un_pts_map = cur_un_pts_map;
}
2.2.5 对新加入的特征点更新全局id
回到feature_tracker_node.cpp文件的img_callback()函数。completed(或者是update())如果是true，说明没有更新完id，则持续循环，如果是false，说明更新完了则跳出循环。注意n_id是static类型的数据，具有累加的功能。
for (unsigned int i = 0;; i++)//更新全局ID
{
bool completed = false;
for (int j = 0; j < NUM_OF_CAM; j++)
if (j != 1 || !STEREO_TRACK)
completed |= trackerData[j].updateID(i);
if (!completed)
break;
}
bool FeatureTracker::updateID(unsigned int i)
{
if (i < ids.size())
{
if (ids[i] == -1)
ids[i] = n_id++;
return true;
}
else
return false;//means all the pts are identified (i meets the end of ids.size())
}
2.2.6 特征点的发布
经过readimg()操作以后，prev-cur-forw帧所有的数据都基本上获取了也对应上了，然后就需要封装到sensor_msgs发布出去，由vins_estimator_node和rviz接收，分别进行后端操作和可视化。注意，这里发布的是cur帧上的所有被观测次数大于1的特征点。在feature_tracker里面，最新一帧是forw，上一帧是cur，上上帧是prev，但是这里发布的特征点是cur帧，所以说这里，能够帮助我们理解为什么这三帧的英语缩写这样写。实际上，forw是最新帧，但是逻辑上，cur才是当前帧。
if (PUB_THIS_FRAME)//如果PUB_THIS_FRAME=1则进行发布
{
pub_count++;
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);//归一化坐标
sensor_msgs::ChannelFloat32 id_of_point;
sensor_msgs::ChannelFloat32 u_of_point;//像素坐标x
sensor_msgs::ChannelFloat32 v_of_point;//像素坐标y
sensor_msgs::ChannelFloat32 velocity_x_of_point;
sensor_msgs::ChannelFloat32 velocity_y_of_point;
feature_points->header = img_msg->header;
feature_points->header.frame_id = "world";
vector<set<int>> hash_ids(NUM_OF_CAM);
for (int i = 0; i < NUM_OF_CAM; i++)
{
auto &un_pts = trackerData[i].cur_un_pts;//归一化坐标
auto &cur_pts = trackerData[i].cur_pts;//像素坐标
auto &ids = trackerData[i].ids;
auto &pts_velocity = trackerData[i].pts_velocity;
for (unsigned int j = 0; j < ids.size(); j++)
{
if (trackerData[i].track_cnt[j] > 1)//只发布追踪次数大于1的特征点
{
int p_id = ids[j];
hash_ids[i].insert(p_id);
geometry_msgs::Point32 p;//归一化坐标
p.x = un_pts[j].x;
p.y = un_pts[j].y;
p.z = 1;
feature_points->points.push_back(p);//归一化坐标
id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
u_of_point.values.push_back(cur_pts[j].x);//像素坐标
v_of_point.values.push_back(cur_pts[j].y);//像素坐标
velocity_x_of_point.values.push_back(pts_velocity[j].x);
velocity_y_of_point.values.push_back(pts_velocity[j].y);
}
}//将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，
}//封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img
feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);
feature_points->channels.push_back(velocity_x_of_point);
feature_points->channels.push_back(velocity_y_of_point);
ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
//如果是第一帧的话，不发布数据skip the first image; since no optical speed on frist image
if (!init_pub)
{
init_pub = 1;
}
else
pub_img.publish(feature_points);
2.2.7 将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match
if (SHOW_TRACK) //将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match
{
ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
//cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
cv::Mat stereo_img = ptr->image;
for (int i = 0; i < NUM_OF_CAM; i++)
{
cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);
for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
{
double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
}
}
//cv::imshow("vis", stereo_img);
//cv::waitKey(5);
pub_match.publish(ptr->toImageMsg());
}
}
