VINS的视觉处理前端的视觉跟踪模块（feature_trackers）。
论文第四章A节（IV. MEASUREMENT PREPROCESSING——A. Vision Processing Front-end）
可以看到其算法主要包括以下内容：
1、对于每一幅新图像，KLT稀疏光流算法对现有特征进行跟踪；
2、检测新的角点特征以保证每个图像特征的最小数目(100-300)；
3、通过设置两个相邻特征之间像素的最小间隔来执行均匀的特征分布；
4、利用基本矩阵模型的RANSAC算法进行外点剔除；(跟踪失败和图像边界外的点
5、对特征点进行去畸变矫正，然后投影到一个单位球面上(对于cata-fisheye camera)，若是针孔相机则将像素坐标直接转换到归一化平面(z=1)上并采用逆畸变模型（k1,k2,p1,p2）去畸变。

视觉处理前端对关键帧的选择将在之后进行讨论。
没有数学推导，直接看代码实现吧！

在这里插入图片描述

初始化相机模型：读取相机内参

1.CATA（卡特鱼眼相机），采用鱼眼相机模型。

2.PINHOLE针孔相机则将像素坐标直接转换到归一化平面(z=1)上并采用逆畸变模型（k1,k2,p1,p2）去畸变。

feature_tracker的输入输出

输入：图像，即订阅了传感器或者rosbag发布的topic：“/cam0/image_raw”
输出：
1、发布topic：“/feature_trackers/feature_img”
即跟踪的特征点图像，主要是之后给RVIZ用和调试用
2、发布topic：“/feature_trackers/feature”
即跟踪的特征点信息，由/vins_estimator订阅并进行优化
3、发布topic：“/feature_trackers/restart”
即判断特征跟踪模块是否出错，若有问题则进行复位，由/vins_estimator订阅

feature_trackers类的成员变量

cv::Mat mask;//图像掩码
cv::Mat fisheye_mask;//鱼眼相机mask，用来去除边缘噪点

    // prev_img是上一次发布的帧的图像数据
    // cur_img是光流跟踪的前一帧的图像数据
    // forw_img是光流跟踪的后一帧的图像数据
    cv::Mat prev_img, cur_img, forw_img;

    vector<cv::Point2f> n_pts;//每一帧中新提取的特征点
    vector<cv::Point2f> prev_pts, cur_pts, forw_pts;//对应的图像特征点
    vector<cv::Point2f> prev_un_pts, cur_un_pts;//归一化相机坐标系下的坐标
    vector<cv::Point2f> pts_velocity;//当前帧相对前一帧特征点沿x,y方向的像素移动速度
    vector<int> ids;//能够被跟踪到的特征点的id
    vector<int> track_cnt;//当前帧forw_img中每个特征点被追踪的时间次数

map<int, cv::Point2f> cur_un_pts_map;
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作为该特征点的id，然后n_id加1

feature_trackers.cpp实现特征点跟踪的函数

1. ros初始化和设置句柄，设置logger级别 (main）

ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

2、parameters.cpp/parameters.h：实现参数的读取和设置

读取如config->euroc->euroc_config.yaml中的一些配置参数

readParameters(n);

3.读取每个相机实例读取对应的相机内参，NUM_OF_CAM=1为单目

 for (int i = 0; i < NUM_OF_CAM; i++) 
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);

4、判断是否加入鱼眼mask来去除边缘噪声

5、订阅话题IMAGE_TOPIC(如/cam0/image_raw)，执行回调函数img_callback

ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

6、发布feature，实例feature_points，跟踪的特征点，给后端优化用
发布feature_img，实例ptr，跟踪的特征点图，给RVIZ用和调试用

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);

子函数细节部分：

1.回调函数 void img_callback(const sensor_msgs::ImageConstPtr &img_msg)

/**
 * @brief   ROS的回调函数，对新来的图像进行特征点追踪，发布
 * @Description readImage()函数对新来的图像使用光流法进行特征点跟踪
 *              追踪的特征点封装成feature_points发布到pub_img的话题下，
 *              图像封装成ptr发布在pub_match下
 * @param[in]   img_msg 输入的图像
 * @return      void
*/

1、判断是否是第一帧if(first_image_flag)

2、判断时间间隔是否正确，有问题则restart

3、发布频率控制，并不是每读入一帧图像，就要发布特征点，通过判断间隔时间内的发布次数
（不发布的时候也是会执行readImage() 读取图像进行处理）

4、将图像编码8UC1转换为mono8

5、单目时：FeatureTracker::readImage() 函数读取图像数据进行处理

trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());

6、更新全局ID

7、如果PUB_THIS_FRAME=1则进行发布
将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img;
将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match

2.图像特征跟踪的主要处理函数

readImage(const cv::Mat &_img, double _cur_time)

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() 对特征点的图像坐标根据不同的相机模型进行去畸变矫正和深度归一化，计算每个角点的速度

1.setMask()函数

该函数主要用于对跟踪点进行排序并去除密集点。
对跟踪到的特征点，按照被追踪到的次数排序并依次选点；使用mask进行类似非极大抑制的方法，半径为30，去掉分部密集的点，使特征点分布均匀

2.rejectWithF()

该函数主要是通过基本矩阵(F)去除外点outliers。首先将将图像坐标畸变矫正后转换为像素坐标，通过cv::findFundamentalMat()计算F矩阵，利用得到的status通过reduceVector()去除outliers 。

3.calcOpticalFlowPyrLK()

调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪，得到forw_pts

status标记了从前一帧cur_img到forw_img特征点的跟踪状态，无法被追踪到的点标记为0

4.goodFeaturesToTrack

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值

注：Shi-Tomasi算法是对Harris角点检测算法的改进，一般会比Harris算法得到更好的角点。Harris 算法的角点响应函数是将矩阵 M 的行列式值与 M 的迹相减，利用差值判断是否为角点。

后来Shi 和Tomasi 提出改进的方法是，若矩阵M的两个特征值中较小的一个大于阈值，则认为他是角点，即： R=min(λ1,λ2)

Harris算法思想：通过图像的局部的小窗口观察图像，角点的特征是窗口沿任意方向移动都会导致图像灰度的明显变化。

5.addPoints();

//添将新检测到的特征点n_pts添加到forw_pts中，id初始化-1,track_cnt初始化为1.

6.undistortedPoints();

//根据不同的相机模型去畸变矫正和转换到归一化坐标系上，计算速度

对角点图像坐标进行去畸变矫正，转换到归一化坐标系上，并计算每个角点的速度。

原文链接：https://blog.csdn.net/qq_42739167/article/details/124690497