VINS-MONO代码解读---前端feature_tracker

feature_tracker是vins的前端，它的目录在src/ feature_tracker下，功能主要是获取摄像头的图像帧，并按照事先设定的频率，把cur帧上满足要求的特征点以sensor_msg::PointCloudPtr的格式发布出去，以便RVIZ和vins——estimator接收。

这个package下面主要包括4个功能：
feature_tracker——包含特征提取/光流追踪的所有算法函数；
feature_tracker_node——特征提取的主入口，负责一个特征处理节点的功能；
parameters——负责读取来自配置文件的参数；
tic_toc——计时器；
注意，还有package.xml和CmakeLists.txt上面定义了所有的外部依赖和可执行文件。

流程图

如下：

节点输入输出

feature_trackers可被认为是一个单独的模块
输入：图像，即订阅了传感器或者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订阅

代码结构

1）feature_trackers.cpp/feature_trackers.h：构建feature_trackers类，实现特征点跟踪的函数
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

2）feature_trackers_node.cpp：main()函数 ROS的程序入口
3）parameters.cpp/parameters.h：实现参数的读取和设置
以使用配置文件config\euroc\euroc_config.yaml为例
4）tic_toc.h：TicToc类，记录时间

代码解读

接下来将从main()函数开始对加粗的函数进行具体解读：

一、feature_trackers_node.cpp

程序入口 int main
1、ros初始化和设置句柄，设置logger级别

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

2、读取如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用和调试用，由/vins_estimator订阅
判断特征跟踪模块是否出错，若有问题则进行复位，由/vins_estimator订阅

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

回调函数 void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
该函数是ROS的回调函数，主要功能包括：readImage()函数对新来的图像使用光流法进行特征点跟踪，并将追踪的特征点封装成feature_points发布到pub_img的话题下，将图像封装成ptr发布在pub_match下。

pub_img.publish(feature_points);
pub_match.publish(ptr->toImageMsg())

void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
   
   
    // 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.通过时间间隔判断相机数据流是否稳定，有问题则restart
    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;
    }
    last_image_time = img_msg->header.stamp.toSec();// 更新上一帧图像时间戳
 
    // 3.发布频率控制，保证每秒钟处理的Image小于FREQ，频率控制在10HZ以内
    // 并不是每读入一帧图像，就要发布特征点
    // 判断间隔时间内的发布次数，（不发布的时候也是会执行readImage() 读取图像进行处理）
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
    {
   
   
        PUB_THIS_FRAME = true;// 发布当前帧
        // 时间间隔内的发布频率十分接近设定频率时，更新时间间隔起始时刻，并将数据发布次数置0
        if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
        {
   
   
            first_image_time = img_msg->header.stamp.toSec();
            pub_count = 0;
        }
    }
    else
        PUB_THIS_FRAME = false;
 
    // 4.将图像编码8UC1转换为mono8,单色8bit,
    //数据结构：
   //构建了CV:Mat与sensor_masg::Image之间的桥梁。
   //http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages 
//注意，img_msg或img都是sensor_msg格式的，我们需要一个桥梁，转换为CV::Mat格式的数据，以供后续图像处理。  

    cv_bridge::CvImageConstPtr ptr;
    if (img_msg->encoding == "8UC1")
    {
   
   
        sensor_msgs::Image img;// ROS图像消息
        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";
        // cv_bridge的toCVCopy函数将ROS图像消息转化为OpenCV图像，
        ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
    }
    else
        ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
 
    cv::Mat show_img = ptr->image;
    TicToc t_r;
    // 5. 重要！！！trackerData[i].readImage读取图像数据
    //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)//单目
            //readImage()函数读取图像数据进行处理
            trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
        else//双目
        {
   
   
            if (EQUALIZE)//判断是否对图像进行自适应直方图均衡化  // 光太亮或太暗，自适应直方图均衡化处理
            {
   
   
                cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
                clahe->apply(ptr->image.rowRange(ROW * i, ROW