VINS-Mono详解（一）

VINS-Mono前端概述

VINS-Mono将前端封装为一个ROS节点feature_tracker，该节点订阅相机图像话题数据后，提取特征点（cv::GoodFeatureToTrack()检测的角点），然后用KLT光流进行特征点跟踪。feature_tracker节点将跟踪的特征点作为话题进行发布，供后端ROS节点使用。同时feature_tracker_node还会发布标记了特征点的图片，可供Rviz显示以供调试。

前端节点的实现在feature_tracker目录下的src中，src里共有3个头文件和3个源文件：

• tic_toc.h中是作者自己封装的一个类TIC_TOC，用来计时；
• parameters.h和parameters.cpp处理前端中需要用到的一些参数；
• feature_tracker.h和feature_tracker.cpp实现了一个类FeatureTracker，用来完成特征点提取和特征点跟踪等主要功能；
• feature_tracker_node.cpp构造了一个ROS节点feature_tracker_node，主要调用FeatureTracker类来实现前端功能。

入口函数main()

前端的入口函数为feature_tracker_node.cpp中的main()函数。在main()函数中，首先创建名为“feature_tracker”的节点，然后调用parameters.cpp中定义的函数readParameters()，读取特征点提取和跟踪需要用到的一些配置参数。

在feature_tracker_node.cpp的开头，main()函数之外，会创建由FeatureTracker类的实例组成的数组trackerData[NUM_OF_CAM]，其中NUM_OF_CAM为相机的个数，这意味这每一个相机都有一个FeatureTracker的实例，每个相机的FeatureTracker实例通过调用成员函数FeatureTracker::readIntrinsicParameter()，来读取每个相机各自对应的内参。

特别的，如果相机是鱼眼相机，需要读取FISHEYE_MASK，存到相机FeatureTracker的实例的成员变量fisheye_mask中，它会在后续操作中被用来去除边缘噪点。

接着定义一个订阅器和两个发布器。订阅器sub_img从话题IMAGE_TOPIC中订阅相机图像数据，回调函数为img_callback()。发布器pub_img在名为feature的话题下发布一条类型为sensor_msgs::PointCloud的消息，该话题消息为从相机图像中跟踪的特征点。发布器pub_match在名为feature_img的话题下发布一条类型为sensor_msgs::Image的消息，该话题消息为标记出了特征点的图像。

	name	topic	type	消息内容
subscriber	sub_img	IMAGE_TOPIC	sensor_msgs::Image	相机图像数据
publisher	pub_img	feature	sensor_msgs::PointCloud	跟踪的特征点
publisher	pub_match	feature_img	sensor_msgs::Image	标记出了特征点的图像

接着便循环等待回调函数，直至程序退出。

回调函数img_callback()

前端的功能主要就在img_callback()，每当接收到从IMAGE_TOPIC话题订阅的数据，就会进入回调函数img_callback()进行处理。

发布频率控制

并不是每处理一帧图像，都将特征点检测跟踪结果发布出去。数据发布频率由配置参数FREQ给定，通过PUB_THIS_FRAME控制是否发布当前帧的检测跟踪数据，将数据平均发布频率稳定在FREQ：如果当前统计时间内的平均数据发布频率快于FREQ，则将PUB_THIS_FRAME置为false，只进行特征点的跟踪，但不发布当前帧的数据；否则，将PUB_THIS_FRAME置为true，进行特征点的跟踪且发布当前帧的数据。

特征点提取与光流跟踪

这一部分代码可处理单目相机和双目相机两种情况。

单目处理逻辑

如果是单目相机（双目开关STEREO_TRACK为0），则只有一个相机：相机0。调用FeatureTracker::readImage()函数，读取单目图像数据，然后在readImage()函数中，对前一帧图像中的特征点进行金字塔光流跟踪，必要时检测新的特征点对特征点数量进行补充。

双目处理逻辑

如果是双目相机（双目开关STEREO_TRACK为1），则有两个相机：相机0和相机1。对于相机0：在readImage()函数中，前后两帧图像之间进行金字塔光流跟踪，必要时在当前帧中检测新特征点以补充特征点数量。对于相机1：如果需要发布当前帧的数据（PUB_THIS_FRAME为true），且相机0的前一帧图像中特征点数量不为空，则直接在回调函数img_callback()中，相机1的当前帧图像对相机0的前一帧图像进行金字塔光流跟踪，这里光流跟踪的处理过程与单目模式下的类似，只是不会补充新的特征点；否则不需要进一步处理。

FeatureTracker::readImage()函数

FeatureTracker类中的主要处理函数就是readImage()，在这个函数中涉及到几个变量名，需要对它们的含义进行特别说明（以下说明针对单目模式，其含义并不适用于双目模式），否则根据变量名称去揣测其含义会出错。

图像数据变量：

• prev_img： 上一次发布数据时对应的图像帧
• cur_img： 光流跟踪的前一帧图像，而不是“当前帧”
• forw_img： 光流跟踪的后一帧图像，真正意义上的“当前帧”

特征点数据变量：

• prev_pts： 上一次发布的，且能够被当前帧（forw）跟踪到的特征点
• cur_pts： 在光流跟踪的前一帧图像中，能够被当前帧（forw）跟踪到的特征点
• forw_pts： 光流跟踪的后一帧图像，即当前帧中的特征点（除了跟踪到的特征点，可能还包含新检测的特征点）

FeatureTracker::readImage()函数的处理流程为：

1. 如果控制参数EQUALIZE为true，调用cv::createCLAHE对图像进行自适应直方图均衡处理；

2. 调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行金字塔光流跟踪，得到forw_pts。status标记了cur_pts中各个特征点的跟踪状态，根据status将跟踪失败的特征点从prev_pts、cur_pts和forw_pts中剔除，而且在记录特征点id的ids，和记录特征点被跟踪次数的track_cnt中，也要把这些跟踪失败的特征点对应位置的记录删除。被status标记为跟踪正常的特征点，在当前帧图像中的位置可能已经处于图像边界外了，这些特征点也应该被删除，删除操作同上。

3. 如果不需要发布当前帧的数据，则直接将当前帧forw的相关数据赋给上一帧cur，然后在这一步整个readImage的流程就结束了。

4. 如果需要发布当前帧的数据，先调用FeatureTracker::rejectWithF()函数，剔除outliers。具体方法为：调用cv::findFundamentalMat()对prev_pts和forw_pts计算F矩阵，通过F矩阵去除outliers。剩下的特征点track_cnt都加1。

5. 调用FeatureTracker::setMask()，通过设置一个mask，使跟踪的特征点在整幅图像中能够均匀分布，防止特征点扎堆。FeatureTracker::setMask()的具体操作为：对光流跟踪到的特征点forw_pts，按照被跟踪到的次数降序排列，然后按照降序遍历这些特征点。每选中一个特征点，在mask中将该点周围半径为MIN_DIST的区域设置为0，后面不再选取该区域内的特征点。这样会删去一些特征点，使得特征点分布得更加均匀，同时尽可能地保留被跟踪次数更多的特征点。

6. 由于光流跟踪到的特征点会减少，而且setMask()的处理过程中也会删除一些特征点，所以需要新检测一些特征点（只有需要发布数据时，才会检测新的特征点，否则只跟踪，不检测新的特征点）。具体操作为：调用cv::goodFeaturesToTrack()在mask中不为0的区域检测新的特征点，将特征点数量补充至指定数量。然后调用FeatureTracker::addPoints()，将新检测到的特征点到forw_pts中去，id初始化为-1，track_cnt初始化为1。

更新特征点id

特征点id相当于特征点的身份证号，对数据关联（data association）至关重要。需要注意的是，更新特征点id的步骤被特意放到了回调函数img_callback()中，而不是FeatureTracker::readImage()函数内部。有一种说法是，n_id是FeatureTracker类的静态变量：

static int n_id;

FeatureTracker类的多个实例对象会共享一个n_id，在readImage()函数内部更新特征点id的话，如果多个相机并行调用readImage()，它们都要去访问n_id并改变它的值，可能会产生问题。我有一个疑问：为什么会出现多个相机并行调用readImage()的情况，因为从源代码来说，可以保证多个相机的调用存在时序上的先后关系。