VINS-Mono 代码解析——视觉跟踪 feature_trackers

参考链接：https://blog.csdn.net/qq_41839222/article/details/86030962
参考链接：https://blog.csdn.net/try_again_later/article/details/104847052
二位博主写的太好了 感谢博主 ~~~O(∩_∩)O哈哈~

理论分析

这部分主要说一下视觉跟踪模块（feature_trackers），先说一下涉及到的具体内容：

• 对于每一幅新图像，KLT稀疏光流算法对现有特征进行跟踪；
• 检测新的角点特征以保证每个图像特征的最小数目(100-300)
• 通过设置两个相邻特征之间像素的最小间隔来执行均匀的特征分布；
• 利用基本矩阵模型的RANSAC算法进行外点剔除；
• 对特征点进行去畸变矫正，然后投影到一个单位球面上(对于cata-fisheye camera)。
• 关键帧选取
  • 当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧（因为视差可以根据平移和旋转共同得到，而纯旋转则导致不能三角化成功，所以这一步需要IMU预积分进行补偿）
  • 当前帧跟踪到的特征点数量小于阈值视为关键帧；

部分理论说明

这里补充一下理论部分的:对特征点进行去畸变矫正，然后投影到一个单位球面上(对于cata-fisheye camera)

m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);

可以根据不同的相机模型将二维坐标转换到三维坐标：
对于CATA（卡特鱼眼相机）将像素坐标投影到单位圆内，这里涉及了鱼眼相机模型
而对于PINHOLE（针孔相机）将像素坐标直接转换到归一化平面（z=1）并采用逆畸变模型(k1,k2,p1,p2)去畸变等。

节点在启动时会先读取相机内参，根据config_file文件中model_type的值决定采用何种相机模型，并创建相应模型的对象指针，读取在该模型下需要的参数。他们之间的调用顺序:
readIntrinsicParameter ------> generateCameraFromYamlFile -----> CameraPtr

for (int i = 0; i < NUM_OF_CAM; i++) 
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);//读取相机内参,保存到CAM_NAMES[i],如何获取相机的内呢???
        
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);
}

CameraPtr
CameraFactory::generateCameraFromYamlFile(const std::string& filename)
{
    cv::FileStorage fs(filename, cv::FileStorage::READ);
    if (!fs.isOpened())
    {
        return CameraPtr();
    }
    Camera::ModelType modelType = Camera::MEI;
    if (!fs["model_type"].isNone())
    {
        std::string sModelType;
        fs["model_type"] >> sModelType;
        if (boost::iequals(sModelType, "kannala_brandt"))
        {
            modelType = Camera::KANNALA_BRANDT;
        }
        else if (boost::iequals(sModelType, "mei"))
        {
            modelType = Camera::MEI;
        }
        else if (boost::iequals(sModelType, "scaramuzza"))
        {
            modelType = Camera::SCARAMUZZA;
        }
        else if (boost::iequals(sModelType, "pinhole"))
        {
            modelType = Camera::PINHOLE;
        }
        else
        {
            std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl;
            return CameraPtr();
        }
    }
    switch (modelType)
    {
    case Camera::KANNALA_BRANDT:
    {
        EquidistantCameraPtr camera(new EquidistantCamera);
        EquidistantCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    case Camera::PINHOLE:
    {
        PinholeCameraPtr camera(new PinholeCamera);
        PinholeCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    case Camera::SCARAMUZZA:
    {
        OCAMCameraPtr camera(new OCAMCamera);
        OCAMCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    case Camera::MEI:
    default:
    {
        CataCameraPtr camera(new CataCamera);
        CataCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    }
    return CameraPtr();
}

数据预处理框架

代码流程如下图：主要三个源程序，feature_tracker_node是特征跟踪线程的系统入口，feature_tracker是特征跟踪算法的具体实现，parameters是设备等参数的读取和存放。


我们有了整体的理论以及部分理论的补充之后,我们看一下视觉跟踪整体的ROS框架

ROS代码实现

feature_trackers可被认为是一个单独的模块
输入：

• 图像，即订阅了传感器或者rosbag发布的topic：“/cam0/image_raw”

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

输出：

• 发布topic：“/feature_trackers/feature_img” 即跟踪的特征点图像，主要是之后给RVIZ用和调试用
• 发布topic：“/feature_trackers/feature” 即跟踪的特征点信息，由/vins_estimator订阅并进行优化
• 发布topic：“/feature_trackers/restart” 即判断特征跟踪模块是否出错，若有问题则进行复位，由/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);

现在我们知道了视觉跟踪的ROS框架, 订阅者和发布者等信息. 接下来一起看一下视觉跟踪的代码结构.

视觉跟踪代码结构

VINS对于视觉跟踪相关的代码在feature_tracker文件夹中。
feature_trackers_node.cpp：main()函数 ROS的程序入口, 进入后配置参数,在parameters.cpp中.


feature_trackers_node.cpp 中的局部代码

 //1.读取yaml中的一些配置参数 地址为config->euroc->euroc_config.yaml
    readParameters(n);
    //2.读取每个相机实例对应的相机内参，NUM_OF_CAM 经常为1，单目
    for (int i = 0; i < NUM_OF_CAM; i++)
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);//读取相机内参,保存到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);

feature_trackers.cpp/feature_trackers.h：构建feature_trackers类，实现特征点跟踪的函数 位于feature_tracker.cpp 与feature_tracker.h中,我们看一下各个函数的功能:

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_node.cpp的回调函数和feature_trackers类中，接下来将从main()函数开始对加粗的函数进行具体解读:

一、feature_tracker_node.cpp

主要分为两部分：int main（）函数为程序入口，void img_callback（）为ROS的回调函数，对图像进行特征点追踪，处理和发布。

int main(int argc, char **argv)
{
    ros::init(argc, argv, "feature_tracker");  //ros初始化和设置句柄
    ros::NodeHandle n("~");// 命名空间为/node_namespace/node_name

    //设置logger的级别。 只有级别大于或等于level的日志记录消息才会得到处理。
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    // 1.读取yaml中的一些配置参数 地址为config->euroc->euroc_config.yaml
    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);
/*
    //创建话题的subscriber
    ros::Subscriber	subscribe(const	string	&topic,	uint32_t	queue_size,	void(*)(M));
    //第一个参数是订阅话题的名称
    //第二个参数是订阅队列的长度,如果受到的消息都没来得及处理,那么新消息入队旧消息就会出队
    //第三个参数是回调函数指针,指向回调函数来处理接收到的消息
 */
    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);
    /*
    if (SHOW_TRACK)
        cv::namedWindow("vis", cv::WINDOW_NORMAL);
    */
   ros::spin();
    return 0;
}

1、void ing_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())

我们从流程图可以看到内部函数之间的调用顺序,先看一下各个函数的功能

 // 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
    // detect unstable camera stream
    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(); // 更新上一帧图像时间戳

    // frequency control
    // 3.发布频率控制，保证每秒钟处理的Image小于FREQ，频率控制在10HZ以内
    // 并不是每读入一帧图像，就要发布特征点
    // 判断间隔时间内的发布次数
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
    {
        PUB_THIS_FRAME = true; // 发布当前帧
        // reset the frequency control
        // 时间间隔内的发布频率十分接近设定频率时，更新时间间隔起始时刻，并将数据发布次数置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;

 //OpenCV中，图像是以Mat矩阵的形式存储的，与ROS定义的图像消息的格式不同，需要利用cv_bridge进行转换
    // 4.将图像编码8UC1转换为mono8,单色8bit
    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将ROS的图像消息转换为OpenCV图像格式时都是通过CvImage类实现的。
        //cv_bridge提供了两种方式转换为CvImage类，分别为复制（copy）和共享（share）
        //   CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source,
        //                       const std::string& encoding = std::string());
        //   CvImagePtr toCvCopy(const sensor_msgs::Image& source,
        //                       const std::string& encoding = std::string());

        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;

在这里函数进入readImage(); 读取图像数据并处理,

  // 5. 重要！！！trackerData[i].readImage读取图像数据
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ROS_DEBUG("processing camera %d", i);
        if (i != 1 || !STEREO_TRACK) //单目
            //cur_img 上一针图像
            //forw_img 当前针图像
            //cur_pts 上一针的点坐标
            //forw_pts 当前针的点坐标
            //ids 每个点的ids
            //track_cnt 每个点被跟踪的次数
            //cur_un_=ts最新针的归一化相机洗坐标
            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 * (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
    }

 // 6.更新全局ID
    for (unsigned int i = 0;; i++)
    {
        bool completed = false;
        for (int j = 0; j < NUM_OF_CAM; j++)
            if (j != 1 || !STEREO_TRACK)
                completed |= trackerData[j].updateID(i); // 更新feature的id
        if (!completed)
            break;
    }

 // 6.更新全局ID
    // 7、将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，
    //封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img;

    if (PUB_THIS_FRAME)
    {
        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;
        sensor_msgs::ChannelFloat32 v_of_point;
        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<set<int>> 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);
                }
            }
        }
        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);
  

// 8、将图像封装到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）
                //显示追踪状态，越红越好，越蓝越不行

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

二、feature_tracker.cpp

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

主要分为两部分：

1、特征点检测：Frame1，goofFeaturesToTrack()检测MAX_CNT个特征点，添加到forw_pts中

2、特征点跟踪：calOpticalFlowPyrLK跟踪，将跟踪失败的点剔除，跟踪成功的跟踪次数+1，调用goodFeaturesToTracl（）再检测出MAX_CNT-forw_pts.size()个特征点，添加到forw_pts中，调用updateID 更新ids。

undistortedPoints()中cur_un_pts为归一化相机坐标系下坐标，pts_velocity为当前帧相对于前一帧特征点沿着x、y方向的像素移动速度。

对图像使用光流法进行特征点跟踪，具体流程有：

①先调用createCLAHE() 对图像进行自适应直方图均衡化(如果EQUALIZE=1，表示太亮或则太暗)

②调用calcOpticalFlowPyrLK()跟踪cur_pts到forw_pts,根据status,把跟踪失败的点剔除(注意:prev, cur,forw, ids, track_cnt都要剔除),这里还加了个inBorder判断,把跟踪到图像边缘的点也剔除掉.

③如果不需要发布特征点,则到这步就完了,把当前帧forw赋给上一帧cur, 然后退出.如果需要发布特征点(PUB_THIS_FRAME=1), 则执行下面的步骤

④先调用rejectWithF()对prev_pts和forw_pts做ransac剔除outlier.(实际就是调用了findFundamentalMat函数), 在光流追踪成功就记被追踪+1，数值代表被追踪的次数，数值越大，说明被追踪的就越久

⑤调用setMask(), 先对跟踪点forw_pts按跟踪次数降排序, 然后依次选点, 选一个点, 在mask中将该点周围一定半径的区域设为0, 后面不再选取该区域内的点. 有点类似与non-max suppression, 但区别是这里保留track_cnt最高的点.

⑥在mask中不为0的区域,调用goodFeaturesToTrack提取新的特征角点n_pts, 通过addPoints()函数push到forw_pts中, id初始化-1,track_cnt初始化为1.

undistortedPoints() 对角点图像坐标去畸变矫正，并计算每个角点的速度

值得注意的是，当前帧为forw_pts,被追踪到的帧；curr_pts实际上是上一帧；

//对图像使用光流法进行特征点跟踪
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;
   // ①先调用createCLAHE() 对图像进行自适应直方图均衡化(如果EQUALIZE=1，表示太亮或则太暗)
    // 1.如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理
    if (EQUALIZE)
    {
        //自适应直方图均衡
        //createCLAHE(double clipLimit, Size tileGridSize)
        //CLAHE 用来对图像进行灰度变换，以达到提高图像对比度的目的。
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//size类中的两个数据成员叫做width和heigh
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

    // 2. 判断当前帧图像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;
    }

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

    if (cur_pts.size() > 0)// 前一帧有特征点
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> 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. 光流追踪成功,特征点被成功跟踪的次数就加1
    //数值代表被追踪的次数，数值越大，说明被追踪的就越久

    for (auto &n : track_cnt)
        n++;
    //PUB_THIS_FRAME=1 需要发布特征点

    if (PUB_THIS_FRAME)
    {
        // 6. rejectWithF()通过基本矩阵剔除outliers
        rejectWithF();
         // 7. setMask()保证相邻的特征点之间要相隔30个像素,设置mask
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        setMask();
        ROS_DEBUG("set mask costs %fms", t_m.toc());

        // 8. 寻找新的特征点 goodFeaturesToTrack()
        ROS_DEBUG("detect feature begins");
        TicToc t_t;

        //计算是否需要提取新的特征点
        int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
        if (n_max_cnt > 0)
        {
            if(mask.empty())
                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();
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        // 9. addPoints()向forw_pts添加新的追踪点
        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;
    prev_un_pts = cur_un_pts;

    //把当前帧的数据forw_img、forw_pts赋给上一帧cur_img、cur_pts
    cur_img = forw_img;
    cur_pts = forw_pts;

    // 11. 根据不同的相机模型去畸变矫正和转换到归一化坐标系上，计算速度
    undistortedPoints();
    prev_time = cur_time;
}

1 inBorder（）函数

//判断跟踪的特特征点是否在图像边界内
bool inBorder(const cv::Point2f &pt)
{
    const int BORDER_SIZE = 1;
    int img_x = cvRound(pt.x);
    int img_y = cvRound(pt.y);
    //cvRound()：返回跟参数最接近的整数值，即四舍五入；
    // cvFloor()：返回不大于参数的最大整数值，即向下取整；
    // cvCeil()：返回不小于参数的最小整数值，即向上取整；
    return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;
}

2 reduceVector()函数

//去除无法跟踪的特征点,status为0的点直接跳过，否则v[j++]=v[i]留下来，最后v.resize(j)根据最新的j安排内存
void reduceVector(vector<cv::Point2f> &v, vector<uchar> status)
{
    int j = 0;
    for (int i = 0; i < int(v.size()); i++)
        if (status[i])
            v[j++] = v[i];
    v.resize(j);//resize()，设置大小（size）; reserve()，设置容量（capacity）;
}

void reduceVector(vector<int> &v, vector<uchar> status)
{
    int j = 0;
    for (int i = 0; i < int(v.size()); i++)
        if (status[i])
            v[j++] = v[i];
    v.resize(j);
}

3 setMask()函数

//对跟踪点进行排序并去除密集点
// 对跟踪到的特征点，按照被追踪到的次数排序并依次选点，使用mask进行类似非极大抑制，半径为30，去掉密集点，
//使特征点分布均匀。对跟踪到的特征点从大到小排序并去除密集的点。
void FeatureTracker::setMask()
{
    // 如果是鱼眼镜头直接clone即可，否则创建空白板
    if(FISHEYE)
        mask = fisheye_mask.clone();
    else
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));

    // 倾向于留下被追踪时间很长的特征点
    // 构造(cnt，pts，id)序列，（追踪次数，当前特征点坐标，id）
    vector<pair<int, pair<cv::Point2f, int>>> 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])));

    // 对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序（lambda表达式）
    sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
         {
            return a.first > b.first;
         });
     //清空cnt，pts，id并重新存入
    forw_pts.clear();
    ids.clear();
    track_cnt.clear();

    for (auto &it : cnt_pts_id)
    {
        if (mask.at<uchar>(it.second.first) == 255)// 这个特征点对应的mask值为255，表明点是黑的，还没占有
        {
            //则保留当前特征点，将对应的特征点位置pts，id，被追踪次数cnt分别存入
            forw_pts.push_back(it.second.first);
            ids.push_back(it.second.second);
            track_cnt.push_back(it.first);

            //在mask中将当前特征点周围半径为MIN_DIST的区域设置为0，后面不再选取该区域内的点（使跟踪点不集中在一个区域上）
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }
}

4 addPoints()函数

void FeatureTracker::addPoints()
{
    for (auto &p : n_pts)
    {
        forw_pts.push_back(p);
        ids.push_back(-1);//新提取的特征点id初始化为-1
        track_cnt.push_back(1);//新提取的特征点被跟踪的次数初始化为1
    }
}

5 rejectWithF()函数

// 通过F矩阵去除outliers
// 首先把特征点转化到归一化相机坐标系，然后计算F矩阵，再根据status清除为0的特征点。
void FeatureTracker::rejectWithF()
{
    if (forw_pts.size() >= 8)// 当前帧（追踪上）特征点数量足够多
    {
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;
        // 1.遍历所有特征点，转化为归一化相机坐标系
        vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
        for (unsigned int i = 0; i < cur_pts.size(); i++)//遍历上一帧所有特征点
        {
            Eigen::Vector3d tmp_p;

            //对于PINHOLE（针孔相机）可将像素坐标转换到归一化平面并去畸变。根据不同的相机模型将二维坐标转换到三维坐标
            m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
            //转换为归一化像素坐标，上一帧和当前帧
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());

            m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
        }

        vector<uchar> status;

        // 2. 调用cv::findFundamentalMat对un_cur_pts和un_forw_pts计算F矩阵，需要归一化相机系，z=1
        cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = cur_pts.size();

        // 3. 根据status删除一些特征点
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(cur_un_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
        ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
    }
}

6 updateID()函数

bool FeatureTracker::updateID(unsigned int i)
{
    if (i < ids.size())
    {
        if (ids[i] == -1)
            ids[i] = n_id++;
        return true;
    }
    else
        return false;
}

7 showUndistortion()函数

//显示去畸变矫正后的特征点  name为图像帧名称
void FeatureTracker::showUndistortion(const string &name)
{
    cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0));
    vector<Eigen::Vector2d> distortedp, undistortedp;
    for (int i = 0; i < COL; i++)
        for (int j = 0; j < ROW; j++)
        {
            Eigen::Vector2d a(i, j);
            Eigen::Vector3d b;
            m_camera->liftProjective(a, b);
            distortedp.push_back(a);
            undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z()));
            //printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z());
        }
    for (int i = 0; i < int(undistortedp.size()); i++)
    {
        cv::Mat pp(3, 1, CV_32FC1);
        pp.at<float>(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2;
        pp.at<float>(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2;
        pp.at<float>(2, 0) = 1.0;
        //cout << trackerData[0].K << endl;
        //printf("%lf %lf\n", p.at(1, 0), p.at(0, 0));
        //printf("%lf %lf\n", pp.at(1, 0), pp.at(0, 0));
        if (pp.at<float>(1, 0) + 300 >= 0 && pp.at<float>(1, 0) + 300 < ROW + 600 && pp.at<float>(0, 0) + 300 >= 0 && pp.at<float>(0, 0) + 300 < COL + 600)
        {
            undistortedImg.at<uchar>(pp.at<float>(1, 0) + 300, pp.at<float>(0, 0) + 300) = cur_img.at<uchar>(distortedp[i].y(), distortedp[i].x());
        }
        else
        {
            //ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, pp.at(1, 0), pp.at(0, 0));
        }
    }
    cv::imshow(name, undistortedImg);
    cv::waitKey(0);
}

8 undistortedPoints()函数

//1.对角点图像坐标进行去畸变矫正，转换到归一化坐标系上，
//2.并计算每个角点的速度。  
void FeatureTracker::undistortedPoints()
{
    cur_un_pts.clear();
    cur_un_pts_map.clear();
    //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
    // 1.归一化相机坐标系
    for (unsigned int i = 0; i < cur_pts.size(); i++)// 遍历所有特征点
    {
        Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
        Eigen::Vector3d b;
         //根据不同的相机模型将二维坐标转换到归一化相机三维坐标系
        m_camera->liftProjective(a, b);

        //再延伸到深度归一化平面上
        cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
        cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));
        //printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
    }

    // 2.计算每个特征点的速度到pts_velocity
    // caculate points velocity
    if (!prev_un_pts_map.empty())// 2.1 地图不是空的判断是否新的点
    {
        double dt = cur_time - prev_time;
        pts_velocity.clear();
        for (unsigned int i = 0; i < cur_un_pts.size(); i++)
        {
            if (ids[i] != -1)// 2.2 通过id判断不是最新的点
            {
                std::map<int, cv::Point2f>::iterator it;// 地图的迭代器
                it = prev_un_pts_map.find(ids[i]);// 找到对应的id

                if (it != prev_un_pts_map.end()) // 2.3 在地图中寻找是否出现过id判断是否最新点
                {
                    double v_x = (cur_un_pts[i].x - it->second.x) / dt;// 当前帧-地图点上一帧
                    double v_y = (cur_un_pts[i].y - it->second.y) / dt;
                    pts_velocity.push_back(cv::Point2f(v_x, v_y));// 之前出现过，push_back即可

                }
                else
                    pts_velocity.push_back(cv::Point2f(0, 0));// 之前没出现过，先放进去但是速度为0
            }
            else
            {
                pts_velocity.push_back(cv::Point2f(0, 0));// 是最新的点，速度为0
            }
        }
    }
    else
    {
        // 如果地图是空的，速度是0
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            pts_velocity.push_back(cv::Point2f(0, 0));
        }
    }
     // 更新地图
    prev_un_pts_map = cur_un_pts_map;
}