VINS-Mono 代码解析一、前端

 

目录

一、前言

二、程序解析

    2.1 参数读取

 2.2 读取相机对应的相机内参

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

2.4   订阅话题

4、对最新帧forw的特征点的提取和光流追踪(核心)

5、对新加入的特征点更新全局id

6、发布到pub_img

7、将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match

三、总结

---

一、前言

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

这个package下面主要包括4个功能：

• feature_tracker                 ——包含特征提取/光流追踪的所有算法函数；
• feature_tracker_node       ——特征提取的主入口，负责一个特征处理结点的功能；
• parameters                       ——负责读取来自配置文件的参数；
• tic_tok                               ——计时器；

注意，还有package.xml和CmakeLists.txt上面定义了所有的外部依赖和可执行文件。

二、程序解析

程序的入口在 feature_tracker_node.cpp  里面，从主函数开始（看程序的一种方法）：

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);
    
    //1.为什么是读取【euroc_config.yaml】中的一些配置参数？答：具体看parameter.cpp
    readParameters(n); 
    //2.读取每个相机实例对应的相机内参，NUM_OF_CAM 经常为1，单目
    for (int i = 0; i < NUM_OF_CAM; i++)
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);

    // 3.判断是否加入鱼眼 mask 来去除边缘噪声
    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");
        }
    }

    // 4.订阅话题IMAGE_TOPIC(/cam0/image_raw),有图像发布到这个话题时，执行回调函数img_callback
    ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
    // 5.发布feature点云，实例feature_points，跟踪的特征点，给后端优化用
    pub_img = n.advertise("feature", 1000);
    //发布feature_img，实例ptr，跟踪的特征点图，给RVIZ用和调试用；
    //开始我也不明白怎么传给rviz的，但是你看一下rviz的配置文件vins_rviz_config.rviz就知道了
    pub_match = n.advertise("feature_img",1000);
    //发布restart，estimator_node.cpp订阅使用
    pub_restart = n.advertise("restart",1000);
 
    ros::spin();
    return 0;
}

   2.1 参数读取

 （1） readParameters(n)

void readParameters(ros::NodeHandle &n)
{
    std::string config_file;
    config_file = readParam(n, "config_file");
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }
    std::string VINS_FOLDER_PATH = readParam(n, "vins_folder");

    fsSettings["image_topic"] >> IMAGE_TOPIC;
    fsSettings["imu_topic"] >> IMU_TOPIC;
    MAX_CNT = fsSettings["max_cnt"];
    MIN_DIST = fsSettings["min_dist"];
    ROW = fsSettings["image_height"];
    COL = fsSettings["image_width"];
    FREQ = fsSettings["freq"];
    F_THRESHOLD = fsSettings["F_threshold"];
    SHOW_TRACK = fsSettings["show_track"];
    EQUALIZE = fsSettings["equalize"];
    FISHEYE = fsSettings["fisheye"];
    if (FISHEYE == 1)
        FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg";
    CAM_NAMES.push_back(config_file);

    WINDOW_SIZE = 20;
    STEREO_TRACK = false;
    FOCAL_LENGTH = 460;
    PUB_THIS_FRAME = false;

    if (FREQ == 0)
        FREQ = 100;

    fsSettings.release();
}

功能：读取 config文件夹中 euroc_config.yaml 文件中的 配置参数

步骤：

•  程序第一步,启动 roslaunch vins_estimator euroc.launch,  进入feature_tracker_node.cpp main()函数
•  节点名字feature_tracker， 参数
•     对应的launch
•  readParam 读取对应文件的参数

 2.2 读取相机对应的相机内参

VINS 细节系列 - readIntrinsicParameter（）_努力努力努力-CSDN博客

void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
    ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}

解释：

FeatureTracker trackerData[NUM_OF_CAM];   //类变量

NUM_OF_CAM  是相机的个数，VINS-mono中用到一个相机 ，故大小等于1，单目

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

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

        }
    }

2.4   订阅话题

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

说明：话题名字是 IMAGE_TOPIC =  /cam0/image_raw，当 rosplay *.bag 时便有topic  /cam0/image_raw 发布，

发布到这个话题时，执行回调函数  img_callback（）， 而主要的程序也是从这个回调函数开始的，之后得到的数据进行发布，用于其他模块的话题订阅，如下：

    //发布feature点云，实例feature_points，跟踪的特征点，给后端优化用
    pub_img = n.advertise("feature", 1000);
    //发布feature_img，实例ptr，跟踪的特征点图，给RVIZ用和调试用；
    //开始我也不明白怎么传给rviz的，但是你看一下rviz的配置文件vins_rviz_config.rviz就知道了
    pub_match = n.advertise("feature_img",1000);
    //发布restart，estimator_node.cpp订阅使用
    pub_restart = n.advertise("restart",1000);
   

下面，我们来看一下 img_callback（） 这个函数！

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.通过时间间隔判断相机数据流是否稳定（频率控制）
    //(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)
    {
        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最大只能是1，因为只要能发布一帧，这个数就会被清0
            pub_count = 0;
        }
    }
    else
        PUB_THIS_FRAME = false;

    // 3.图像的格式调整和图像读取
    //读取sensor_msgs::Image img的数据，并转为MONO8格式，用cv::Mat show_img接收。
     
       cv_bridge::CvImageConstPtr ptr;
    if (img_msg->encoding == "8UC1") //8位灰度图像 
    {
        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图像
    /*ROS以自己的sensor_msgs / Image消息格式传递图像，但许多用户希望将图像与OpenCV结合使用。 CvBridge是一个ROS库，提供ROS和OpenCV之间的接口。 可以在vision_opencv stack的cv_bridge包中找到CvBridge；
        将图像编码8UC1转换为mono8,即存储下来的图像为单色，8Bit的图片，一般是bmp，jpeg等*/
        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;
     /*【注意】数据结构：
        构建了CV:Mat与sensor_masg::Image之间的桥梁。
         http://wiki.ros.org/cv_bridge/Tutorials/Using CvBridgeToConvert Between ROSImages And OpenCVImages 
         注意，img_msg或img都是sensor_msg格式的，我们需要一个桥梁，转换为CV::Mat格式的数据，以供后续图像处理。*/
    
    
    // 4. 对最新帧forw的特征点的提取和光流追踪(核心)
    /*img_callback()最最核心的语句出现在这里，也就是readImage()，这个函数实现了特征的处理和光流的追踪，里面基本上调用了feature_tracker.cpp里面的全部函数。在这里首先进行了单目和双目的判断，由于VINS-Mono是单目的，所以后部分关于双目的判断不用看，作者写在这里我觉得是为VINS-Fusion做准备的。在这里，trackerData[i]再次出现了(见3.1)。重要，trackerData[i].readImage读取图像数据；readImage()传了2个参数，当前帧的图像和当前帧的时间戳,以下内容见feature_tracker.cpp，看一下原函数里面是怎么写的*/
    
    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)//（1）判断是否对图像进行自适应直方图均衡化
            {
                cv::Ptr clahe = cv::createCLAHE();
                clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
            }
            else
                trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
        }

#if SHOW_UNDISTORTION
        trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
#endif
    }

    //5.对新加入的特征点更新全局id
    /*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;
    }

    // 6、将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，
    //封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img;
    /*经过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++;
        // 重要，feature_points
        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> hash_ids(NUM_OF_CAM);    // 哈希表id
        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);// 哈希表id  insert
                    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);

        
        // 7、将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match
        if (SHOW_TRACK)
        {
            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);// show_img灰度图转RGB（tmp_img）

                //显示追踪状态，越红越好，越蓝越不行---cv::Scalar决定的
                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());
        }
    }
    ROS_INFO("whole feature tracker processing costs: %f", t_r.toc());
}

说明：
ROS的回调函数，主要功能包括：readImage()函数对新来的图像使用光流法进行特征点跟踪，并将追踪的特征点封装成feature_points发布到pub_img的话题下，将图像封装成ptr发布在pub_match下。
数据结构：

•    init_feature             0:   当前是第一帧   1:当前不是第一帧；
•    first_image_flag     0:   当前是第一帧   1:当前不是第一帧；
•    pub_count             每隔delta_t = 1/FREQ 时间内连续(没有中断/没有报错)发布的帧数
•    first_image_time   每隔delta_t = 1/FREQ 时间的帧对应的时间戳；
•    last_image_time   当前帧或上一帧的时间戳；
•    restart_flag            0:     不重启。  1:重启(std_msgs::Bool)；
•    FREQ                      发布特征点的频率(也就是bk/ck帧与bk+1/ck+1帧之间的频率)，注意，FREQ要比实际的照片频率要慢；
•    init_pub                  0:     第一帧不把特征发布到buf里    1:发布到buf里 ；

分析： 我们可以很容易理解 1、 2 、3部分，4部分才是整个前端的核心内容；我注意到 第三部分的内容，其中有一段这样的程序：

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

这个程序的意思是：时间间隔内发布频率十分接近设定频率时，更新时间间隔起始时刻，并将数据发布次数置0，FREQ 是发布跟踪结果的频率（Hz）；

假设：图片都是 0.05s 进来一次，pub_count 初值 = 1， 1/0.05 = 20HZ,    FREQ =30HZ,  此时，上面的程序不执行！pub_count++执行；

pub_count = 2，又进来一张图片，2/(0.05+0.05) = 20HZ , 上面的程序还是不执行；pub_count = 3，3/(0.05+0.05+0.05) = 20HZ , 程序还是不执行，...那么上面的程序就没有意义了吗？ 换句话说，如果图片正常读取，时间间隔不变，这个程序无意义！那图片不正常呢？感觉是针对这个原因设置的！

下面重点看第四部分

对最新帧forw的特征点的提取和光流追踪(核心)

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)//（1）判断是否对图像进行自适应直方图均衡化
            {
                cv::Ptr clahe = cv::createCLAHE();
                clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
            }
            else
                trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
        }

分析：

img_callback()最最核心的语句 readImage()，这个函数实现了特征的处理和光流的追踪，里面基本上调用了 feature_tracker.cpp  里面的全部函数。在这里首先进行了单目和双目的判断，由于VINS-Mono是单目的，所以后部分关于双目的判断不用看，作者写在这里我觉得是为VINS-Fusion做准备的。在这里，trackerData[i]再次出现了。重要trackerData[i].readImage读取图像数据；readImage() 传了2个参数，当前帧的图像和当前帧的时间戳,以下内容见feature_tracker.cpp，看一下原函数里面是怎么写的

子程序：readImage()  

 
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;

    // 1.如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理
    if (EQUALIZE) //自适应直方图均衡化
    {
        //对图像进行自适应直方图均衡化
        cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

    // 2. 判断当前帧图像forw_img是否为空 == 首帧判断和对的forw_img更新
    if (forw_img.empty())
    {
        //如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据
        //将读入的图像赋给当前帧forw_img，同时还赋给prev_img、cur_img
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        //否则，说明之前就已经有图像读入，只需要更新当前帧forw_img的数据
        forw_img = img;
    }

    //3.此时forw_pts还保存的是上一帧图像中的特征点，所以把它清除
    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 status;
        vector err;
        
        // 3. 调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪，得到forw_pts
        //status标记了从前一帧cur_img到forw_img特征点的跟踪状态，无法被追踪到的点标记为0
        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++)
            if (status[i] && !inBorder(forw_pts[i]))// 将当前帧跟踪的位于图像边界外的点标记为0
                status[i] = 0;
            
        // 4. 根据status,把跟踪失败的点剔除
        //不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts、prev_pts和cur_pts中剔除
        //prev_pts和cur_pts中的特征点是一一对应的
        //记录特征点id的ids，和记录特征点被跟踪次数的track_cnt也要剔除
        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. 更新当前特征点被追踪到的次数，数据结构track_cnt见3.1.。
    for (auto &n : track_cnt)//光流追踪成功,特征点被成功跟踪的次数就加1
        n++;                 //数值代表被追踪的次数，数值越大，说明被追踪的就越久
    
    //6.通过基本矩阵剔除外点rejectWithF();PUB_THIS_FRAME=1 需要发布特征点
    if (PUB_THIS_FRAME)
    {
        rejectWithF(); //rejectWithF()通过基本矩阵剔除outliers
        
        
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        setMask(); //对跟踪点进行排序并去除密集点
        
        // 寻找新的特征点 goodFeaturesToTrack()
        ROS_DEBUG("set mask costs %fms", t_m.toc());

        ROS_DEBUG("detect feature begins");
        TicToc t_t;
        //计算是否需要提取新的特征点
        int n_max_cnt = MAX_CNT - static_cast(forw_pts.size());
        if (n_max_cnt > 0)
        {
            if(mask.empty())//在mask中不为0的区域,
                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;
            
            /*
             *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值
             *)   
             */
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        // 9. addPoints()向forw_pts添加新的追踪点
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        ROS_DEBUG("add feature begins");
        TicToc t_a;
        
        //添将新检测到的特征点n_pts添加到forw_pts中，id初始化-1,track_cnt初始化为1.
        addPoints();
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }
    
    // 10. 数据的更新和归一化坐标的获取，更新帧、特征点
    //当下一帧图像到来时，当前帧数据就成为了上一帧发布的数据
    prev_img = cur_img;
    prev_pts = cur_pts;
    
    //把当前帧的数据forw_img、forw_pts赋给上一帧cur_img、cur_pts
    prev_un_pts = cur_un_pts;
    cur_img = forw_img;
    cur_pts = forw_pts;
    
    // 11. 根据不同的相机模型去畸变矫正和转换到归一化坐标系上，计算速度
    undistortedPoints();//这块需要小心，undistortedPoints()函数藏在这里，而这个函数不可以缺少。
    prev_time = cur_time;
    
    /*这个函数干了2件事，第一个是获取forw时刻去畸变的归一化坐标(这个是要发布到rosmsg里的points数据)，另一个是获取forw时刻像素运动速度。为什么是forw时刻的？因为前面有cur_pts = forw_pts，当前还是forw时刻！所以说feature_tracker_node发布的其实是forw的数据！*/
    
/*rejectWithF()主要是利用了cv::findFundamentalMat()这个函数来进一步剔除outlier。这个函数的功能对应在《SLAM十四讲》第七讲2D-2D对极几何相关知识点，两帧上的一系列对应的特征点能够复原出两帧之间的相对位姿变化，也就是基础矩阵E。但是这些特征点中肯定会有一些outlier，所以通过这个opencv的函数，能够巧妙地剔除这些outlier。补充一点，如果函数传入的是归一化坐标，那么得到的是本质矩阵E，如果传入的是像素坐标，那么得到的是基础矩阵。*/
    
}

 参数说明：

•     cv::Mat mask;                   //图像掩码
•     cv::Mat fisheye_mask;  //鱼眼相机mask，用来去除边缘噪点
•     cv::Mat prev_img, cur_img, forw_img;  //是上上次发布的帧的图像数据/光流跟踪的上一帧的图像数据/当前的图像数据
•     vector n_pts;  //每一帧中新提取的特征点
•     vector prev_pts, cur_pts, forw_pts;  //对应的图像特征点
•     vector prev_un_pts, cur_un_pts;  //归一化相机坐标系下的坐标
•     vector pts_velocity;  //当前帧相对前一帧特征点沿x,y方向的像素移动速
•     vector ids;  //能够被跟踪到的特征点的id
•     vector track_cnt;  //当前帧forw_img中每个特征点被追踪的时间次数
•     map cur_un_pts_map;   //构建id与归一化坐标的id，见undistortedPoints()
•     map prev_un_pts_map;
•     camodocal::CameraPtr m_camera;  //相机模型
•     double cur_time;
•     double prev_time;
•     static int n_id;  //用来作为特征点id，每检测到一个新的特征点，就将++n_id作为该特征点   
   

readImage（）内部程序分析

（1）直方图均衡化处理

    // 1.如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理
    if (EQUALIZE) 
    {
        //对图像进行自适应直方图均衡化
        cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;  

 概念：直方图均衡化处理的 “中心思想” 是把原始图像的灰度直方图从比较集中的某个灰度区间变成在全部灰度范围内的均匀分布。直方图均衡化就是对图像进行非线性拉伸，重新分配图像像素值，使一定灰度范围内的像素数量大致相同。直方图均衡化就是把给定图像的直方图分布改变成“均匀”分布直方图分布。

下图为直方图均衡化的过程，体现了“均衡”的含义：（概率密度的均匀）

（2）、（3）图像迭代和特征点清空

    // 2. 判断当前帧图像forw_img是否为空 == 首帧判断和对的forw_img更新
    if (forw_img.empty())
    {
        //如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据
        //将读入的图像赋给当前帧forw_img，同时还赋给prev_img、cur_img
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        //否则，说明之前就已经有图像读入，只需要更新当前帧forw_img的数据
        forw_img = img;
    }

    //3.此时forw_pts还保存的是上一帧图像中的特征点，所以把它清除
    forw_pts.clear();

理解：这段程序很好理解，主要是图像的迭代和特征点的清空，也是为后面的程序作准备！

（4）图像特征点提取和跟踪（重点）

很多函数就不一一展示了，下面说一下大体的流程：

这一部分谢谢爱宝宝的菜鸟: 1.在代码中分析VINS---图解feature_tracker.cpp_liuzheng1的博客-CSDN博客

虽然   cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);  光流函数在 前面，但是开始并没有被执行，因为第一帧还没有特征点，需要提取特征点！故程序执行 cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask); 才把特征点提取出来！循环一下程序即可！

1. 第一帧图像进来

当第一帧图像进来时，执行函数

cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask)

• forw_img 表示当前图像(这里表示第一帧图像)；
• n_pts  表示存储的角点坐标集合；
• MAX_CNT - forw_pts.size() 表示要检测的角点个数，在这里为MAX_CNT=150;

下图表示提取的150个角点，存放到n_pts




2.角点集合处理

在步骤1里检测到角点(n_pts)后,通过函数 addPoints() 是将n_pts中的角点放到forw_pts中，ids表示每个角点的编号，track_cnt表示每个角点的跟踪次数，addPoints()是将检测到的新的角点ID初始化和跟踪次数初始化如下图所示：

3.迭代处理   

    prev_img = cur_img;//在第一帧处理中还是等于当前帧forw_img
    prev_pts = cur_pts;//在第一帧中不做处理
    prev_un_pts = cur_un_pts;//在第一帧中不做处理
    cur_img = forw_img; //将当前帧赋值给上一帧
    cur_pts = forw_pts;//如下图所示

4.第二帧图像进来

第二帧图像forw_img与上一帧图像cur_img进行光流跟踪

cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3) 

• cur_pts表示步骤3中存储的角点
• forw_img表示在当前图像中跟踪成功的点的集合
• status表示 cur_pts和forw_pts中对应点对是否跟踪成功

如下图所示：

光流跟踪结束后，还要判断跟踪成功的角点是否都在图像内（这个不难理解），源码：

for (int i = 0; i < int(forw_pts.size()); i++)
            if (status[i] && !inBorder(forw_pts[i]))
                status[i] = 0;

如图所示：

5.对跟踪成功的点及其对应的id和track_cnt进行重组

对步骤5中的点根据状态status进行重组，将staus中为1的对应点对在原点对中保存下来，为0的对应点对去除掉，对应代码：

        //将光流跟踪后的点的集合，根据跟踪的状态(status)进行重组
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);

        //将光流跟踪后的点的id和跟踪次数，根据跟踪的状态(status)进行重组
        reduceVector(ids, status);
        reduceVector(cur_un_pts, status);
        reduceVector(track_cnt, status);

如图所示：

因为在第二张图像中成功跟踪了四个点(p1,p6,p8,p150)，所以要对该点对应的跟踪次数(track_cnt)进行加1操作，代码：

  //将track_cnt中的每个数进行加一处理，代表又跟踪了一次
    for (auto &n : track_cnt)
        n++;

6.去除光流跟踪错误的点对

该部分用到的函数主要是 rejectWithF();借助基础矩阵计算函数cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);来去除去匹配点，然后在根据状态量staus对匹配的点对，与步骤5进行相似的操作。

7.设置角点检测函数的mask

该部分用到的函数主要是setMask();，该函数主要有两个作用，
1.对跟踪到的特征点对依据跟踪次数进行从多到少的排序，并放到原集合中，对应代码：

 // prefer to keep features that are tracked for long time
    //保存长时间跟踪到的特征点
    //vector>> cnt_pts_id
    vector>> 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> &a, const pair> &b)
         {
            //降序排列
            return a.first > b.first;
         });

    forw_pts.clear();
    ids.clear();
    track_cnt.clear();
    for (auto &it : cnt_pts_id)
    {
        if (mask.at(it.second.first) == 255) //检测新建的mask在该点是否为255
        {
            //将跟踪到的点按照跟踪次数重新排列，并返回到forw_pts，ids，track_cnt
            forw_pts.push_back(it.second.first);
            ids.push_back(it.second.second);
            track_cnt.push_back(it.first);
            //图片，点，半径，颜色为0表示在角点检测在该点不起作用,粗细（-1）表示填充
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }

2.在已跟踪到角点的位置上，将mask对应位置上设为0，意为在cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);进行操作时在该点不再重复进行角点检测，这样可以使角点分布更加均匀。

8. 返回步骤2进行循环处理

再一次进行角点检测，只不过这次角点检测的角点个数会改变，变为MAX_CNT - forw_pts.size()

9.畸变矫正

从第二张图像输入后每进行一次循环，最后还需要对匹配的特征点对进行畸变矫正，主要函数为undistortedPoints();  

5、对新加入的特征点更新全局id

    /*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;
    }

6、发布到pub_img

• 将特征点id，矫正后归一化平面的3D点 (x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img;
•  经过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++;
        // 重要，feature_points
        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> hash_ids(NUM_OF_CAM);    // 哈希表id
        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);// 哈希表id  insert
                    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);

7、将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match

if (SHOW_TRACK)
        {
            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);// show_img灰度图转RGB（tmp_img）

                //显示追踪状态，越红越好，越蓝越不行---cv::Scalar决定的
                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);
                    //draw speed line
                    /*
                    Vector2d tmp_cur_un_pts (trackerData[i].cur_un_pts[j].x, trackerData[i].cur_un_pts[j].y);
                    Vector2d tmp_pts_velocity (trackerData[i].pts_velocity[j].x, trackerData[i].pts_velocity[j].y);
                    Vector3d tmp_prev_un_pts;
                    tmp_prev_un_pts.head(2) = tmp_cur_un_pts - 0.10 * tmp_pts_velocity;
                    tmp_prev_un_pts.z() = 1;
                    Vector2d tmp_prev_uv;
                    trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, tmp_prev_uv);
                    cv::line(tmp_img, trackerData[i].cur_pts[j], cv::Point2f(tmp_prev_uv.x(), tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), 1 , 8, 0);
                    */
                    //char name[10];
                    //sprintf(name, "%d", trackerData[i].ids[j]);
                    //cv::putText(tmp_img, name, trackerData[i].cur_pts[j], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
                }
            }
            //cv::imshow("vis", stereo_img);
            //cv::waitKey(5);
            pub_match.publish(ptr->toImageMsg());
        }
    }
    ROS_INFO("whole feature tracker processing costs: %f", t_r.toc());