VINS-Mono源码解析（二）前端：特征跟踪

　　VINS-Mono中的前端处理在ROS节点feature_tracker_node中，具体实现是GoodFeatureToTrack + LK光流跟踪。

1. 节点概览

　　该节点的功能是：接收图像数据，进行角点提取和光流跟踪，输出跟踪的特征点（角点）。feature_tracker_node的消息订阅发布如下表：

  topic type note Subscribe image sensor_msgs::ImageConstPtr 相机原始图 Publish feature sensor_msgs::PointCloud 跟踪的特征点，给后端优化用 Publish feature_img sensor_msgs::Image 跟踪特征点图片，输出给RVIZ，调试用

2. 代码细节

a. img_callback()

　　接收到image msg后，会进入回调函数img_callback()，每个相机都有一个FeatureTracker实例，即trackerData[NUM_OF_CAM]，回调函数中会调用每个相机实例中的readImage()方法来提取和跟踪特征点，然后将所有相机的特征点合入到feature_points(sensor_msgs::PointCloudPtr)中发布。
　　回调函数中，有一部分逻辑是处理双目的，STEREO_TRACK开关控制，如果STEREO_TRACK置为１，则相机０和相机１是双目，相机０调用FeatureTracker进行跟踪，相机１通过LK光流跟踪相机０中的特征点，处理逻辑和前后帧跟踪逻辑一致，只是不再提取新的特征点。
　　此外，还有一个发布频率控制，并不是每一帧图像都要发布特征点，通过判断间隔时间内发布的次数来控制发布频率，如果需要发布则PUB_THIS_FRAME置位１。如果PUB_THIS_FRAME为０，则只做特征跟踪但不发布．

b. FeatureTracker::readImage()

　　FeatureTracker类中处理的主函数就是readImage()，这里涉及到3个img(prev_img, cur_img, forw_img)和pts(prev_pts, cur_pts, forw_pts)，刚开始看不是太好理解，cur和forw分别是LK光流跟踪的前后两帧，forw才是真正的”当前”帧，cur实际上是上一帧，而prev是上一次发布的帧，它实际上是光流跟踪以后，prev和forw根据Fundamental Matrix做RANSAC剔除outlier用的，也就是rejectWithF()函数. readImage()的处理流程为:
1. 先调用cv::CLAHE对图像做直方图均衡化(如果EQUALIZE打开)
2. 调用calcOpticalFlowPyrLK()跟踪cur_pts到forw_pts,根据status,把跟踪失败的点剔除(注意:prev, cur, forw, ids, track_cnt都要剔除),这里还加了个inBorder判断,把跟踪到图像边缘的点也剔除掉.
3. 如果不需要发布特征点,则到这步就完了,把当前帧forw赋给上一帧cur, 然后退出.
如果需要发布特征点(PUB_THIS_FRAME=1), 则:
4. 先调用rejectWithF()对prev_pts和forw_pts做ransac剔除outlier.(实际就是调用了findFundamentalMat函数), 完了以后, 剩下的点track_cnt都加1.
5. 调用setMask(), 先对跟踪点forw_pts按跟踪次数降排序, 然后依次选点, 选一个点, 在mask中将该点周围一定半径的区域设为0, 后面不再选取该区域内的点. 有点类似与non-max suppression, 但区别是这里保留track_cnt最高的点.
6. 在mask中不为0的区域,调用goodFeaturesToTrack提取新的角点n_pts, 通过addPoints()push到forw_pts中, id初始化-1, track_cnt初始化为1.

完了以后,在外面主回调函数中调用updateID()来更新那些新特征点的ID, 这里其实有点疑问,为什么一定要放到外面? 感觉是基于线程安全性的考虑, 因为n_id是一个static成员变量, 多个相机共享一个n_id, 在readImage()里面更新的话, 如果多个相机并行调用readImage(),可能会导致不同相机的feature_id相同, 不知道是不是这个原因?