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 * (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;
    }
 
    // 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());
        
        // 第一帧不发布，因为没有光流速度
        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_trackers_node.cpp

1.inBorder（）函数

bool inBorder(const cv::Point2f &pt)
{
    const int BORDER_SIZE = 1;
    //cvRound()：返回跟参数最接近的整数值，即四舍五入；
    int img_x = cvRound(pt.x);
    int img_y = cvRound(pt.y);
    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);
}

3.setMask() 函数
对跟踪到的特征点，按照被追踪到的次数排序并依次选点，使用mask进行类似非极大抑制，半径为30，去掉密集点，使特征点分布均匀。对跟踪到的特征点从大到小排序并去除密集的点。

注意，这个函数的操作对象是forw_pts！这里，需要再次明确一下forw_pts里面是什么，它是通过光流法在当前帧中仍然能追踪到的那些特征点在当前帧中的像素坐标！所以forw_pts里面放着的都是老特征点。这里干了2件事，首先对他们根据追踪到的次数进行排序，从而确定了优先级；然后再把特征点周围的密集的特征点剔除掉。所以说，排序的作用就是为了给 剔除操作 提供优先级信息！因为追踪次数多的点在前面，所以在剔除的时候，这些时间长的特征点能够保留下来，而在这些老特征点周围比较近的距离内那些比较新的特征点就剔除掉，从而一方面保证了特征点的稳定，另一方面让特征点的分布更加均匀。
————————————————
版权声明：本文为CSDN博主「iwander。」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/iwanderu/article/details/104618993

void FeatureTracker::setMask()
{
    // 如果是鱼眼镜头直接clone即可，否则创建空白板
    if(FISHEYE)
        mask = fisheye_mask.clone();
    else
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));// 空白板都是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;// a.first指的追踪次数track_cnt
         });
 
    //清空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() 函数
添将新检测到的特征点n_pts，新提取的特征点id初始化为-1，新提取的特征点被跟踪的次数初始化为1。

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.图像特征跟踪的主要处理函数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;
 
    // 1.如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理
    if (EQUALIZE)
    {
        //自适应直方图均衡
        //createCLAHE(double clipLimit, Size tileGridSize)
        cv::Ptr<cv::CLAHE> 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是否为空
    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())//在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();
        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;
}

6.rejectWithF() 函数
通过F矩阵去除outliers，首先把特征点转化到归一化相机坐标系，然后计算F基本矩阵，再根据status清除为0的特征点。（该函数主要是通过基本矩阵（F）去除外点outliers。首先将将图像坐标畸变矫正后转换为像素坐标，通过cv::findFundamentalMat()计算F矩阵，利用得到的status通过reduceVector()去除outliers 。）

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

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<float>(1, 0), p.at<float>(0, 0));
        //printf("%lf %lf\n", pp.at<float>(1, 0), pp.at<float>(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<float>(1, 0), pp.at<float>(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
    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;
}