GVINS代码学习记录2

特征跟踪分为feature_tracker_node.cpp,feature_tracker.cpp,feature_tracker.h三个文件

头文件，介绍特征跟踪器的类跟变量

#pragma once
//定义了一个 FeatureTracker 类和一些辅助函数
#include <cstdio>
#include <iostream>
#include <queue>
#include <execinfo.h>
#include <csignal>

#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>

#include "camodocal/camera_models/CameraFactory.h"
#include "camodocal/camera_models/CataCamera.h"
#include "camodocal/camera_models/PinholeCamera.h"

#include "parameters.h"
#include "tic_toc.h"

using namespace std;
using namespace camodocal;
using namespace Eigen;
//辅助函数，用于检查给定的二维点是否在图像边界内。
bool inBorder(const cv::Point2f &pt);
//辅助函数，用于根据给定的状态向量，从给定的向量中删除对应位置为零的元素
void reduceVector(vector<cv::Point2f> &v, vector<uchar> status);
void reduceVector(vector<int> &v, vector<uchar> status);
//特征跟踪器类，用于在图像序列中跟踪特征点
class FeatureTracker
{//包含了一系列方法和成员变量，用于读取图像、设置掩码、添加特征点、更新特征点ID、读取内参参数等操作
  public:
    FeatureTracker();

    void readImage(const cv::Mat &_img,double _cur_time);//读取新的图像，并将其存储为当前帧图像。

    void setMask();//设置掩码，以过滤掉图像中不需要进行特征跟踪的区域。

    void addPoints();//添加新的特征点到跟踪器中

    bool updateID(unsigned int i);//更新给定索引位置的特征点的ID。

    void readIntrinsicParameter(const string &calib_file);//读取相机的内参参数，以便后续进行去畸变操作

    void showUndistortion(const string &name);//显示去畸变后的图像。

    void rejectWithF();//使用基础矩阵 F 进行外点剔除。

    void undistortedPoints();//对特征点进行去畸变操作。
//成员变量包括当前图像、前一帧图像、下一帧图像、特征点位置、特征点ID等。
    cv::Mat mask;
    cv::Mat fisheye_mask;
    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;
    vector<int> ids;
    vector<int> track_cnt;
    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;
};

feature_tracker_node

main（）

1.读取配置文件，执行readParameters(n)

2,根据相机数量循环读取每个相机的内参，执行readInstrinsicParameter(CAM_NAMES[i])

3,订阅图像话题，执行img_callback

4,调用ros::spin()进入ROS的事件循环，等待接收和处理消息

int main(int argc, char **argv)
{
    ros::init(argc, argv, "feature_tracker");//初始化ros节点，并读取参数
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    readParameters(n);

    for (int i = 0; i < NUM_OF_CAM; i++)
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);//根据相机数量循环读取每个相机的内参信息

    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");
        }
    }
//订阅图像话题IMAGE_TOPIC并指定回调函数img_callback
    ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
   //在回调函数img_callback中，首先判断是否需要显示跟踪结果。
   //创建了 ROS 发布器（pub_img、pub_match、pub_restart）
   //用于发布特征点云、特征点图像和重启信号等消息
    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);
    /*如果需要显示跟踪结果，还创建了一个窗口（vis）用于显示图像
    if (SHOW_TRACK)
        cv::namedWindow("vis", cv::WINDOW_NORMAL);
    */
   //通过调用 ros::spin() 进入 ROS 的事件循环，等待接收和处理消息
    ros::spin();
    return 0;
}

img_callback()

1、操作第一帧图像，记录时间
2、检查时间戳是否正常。若不正常，清空状态，发布重启消息
3、将输入图像转换为单通道灰度图 show_img
4、对于每个相机，读取图像并进行特征点跟踪
5、特征点跟踪提取操作,执行trackerData[i].readImage 函数读取图像并更新特征点跟踪器
6、给新特征点赋予ID，执行trackerData[j].updateID 函数来更新特征点的 ID
7、向后端发布特征点数据

8，整个特征点云消息发布的过程结束后，会输出整个特征跟踪处理的耗时信息

void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    if(first_image_flag)
    {//如果是第一张图像，设置为false，并记录第一张图像的时间戳，并返回
        first_image_flag = false;
        first_image_time = img_msg->header.stamp.toSec();
        last_image_time = img_msg->header.stamp.toSec();
        return;
    }
    // detect unstable camera stream，检测相机图像流是否稳定。
    //通过比较当前图像的时间戳与上一张图像的时间戳，如果时间戳超过1秒或时间戳倒退，则认为图像流不稳定
    //此时，会将first_image_flag设置为true,重置跟踪器，并发布重启信号到pub_start话题中
    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();//如果图像流稳定，将当前图像的时间戳赋值给last_iamge_time
    // frequency control,根据设定的发布频率FREQ进行频率控制
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
    {//通过计算当前已发布的帧数与时间间隔，判断是否满足发布频率要求
    //如果满足要求，将 PUB_THIS_FRAME 设置为 true，并在达到设定频率时重置 first_image_time 和 pub_count
        PUB_THIS_FRAME = true;
        // reset the frequency control
        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
        PUB_THIS_FRAME = false;

    cv_bridge::CvImageConstPtr ptr;//根据图像消息的编码格式进行转换，将输入的图像消息转换为单通道灰度图 show_img
    if (img_msg->encoding == "8UC1")
    {
        sensor_msgs::Image img;
        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";
        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;//对于每个相机，读取图像并进行特征点跟踪
    for (int i = 0; i < NUM_OF_CAM; i++)
    {//如果不是双目跟踪或者当前相机不是右目，则调用 trackerData[i].readImage 函数读取图像并更新特征点跟踪器
        ROS_DEBUG("processing camera %d", i);
        if (i != 1 || !STEREO_TRACK)
            trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
        else
        {
            if (EQUALIZE)//如果启用了直方图均衡化（EQUALIZE 为真），则对图像进行直方图均衡化处理，并将结果保存在 trackerData[i].cur_img 中。
            {
                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
    }
//通过循环遍历来更新特征点的 ID
//在每次迭代中，对于每个相机，调用 trackerData[j].updateID 函数来更新特征点的 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);
        if (!completed)
            break;//如果没有特征点需要更新，则退出循环。
    }
//特征点云消息的发布函数
   if (PUB_THIS_FRAME)
   {//创建并
        pub_count++;
        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);//创建了一个 sensor_msgs::PointCloud 类型的指针 feature_points
        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;
    //声明了一些通道(channel)变量
   // /如果不是第一帧图像，则发布 feature_points，并根据需要显示特征点追踪效果根据输入的图像消息(img_msg)进行特征点的处理，并将处理后的特征点以及相关信息填充到 feature_points 中
        feature_points->header = img_msg->header;
        feature_points->header.frame_id = "world";

        vector<set<int>> hash_ids(NUM_OF_CAM);
        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)
                {
                    int p_id = ids[j];
                    hash_ids[i].insert(p_id);
                    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，并根据需要显示特征点追踪效果。
        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);

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

                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());
}
//整个特征点云消息发布的过程结束后，会输出整个特征跟踪处理的耗时信息。

readImage()

读取新的图像、光流跟踪特征点、添加新的特征点、去畸变处理等。

1、均衡化图像，执行(EQUALIZE)

2，检查是否存在前一帧图像 (forw_img)

3，如果当前特征点的数量大于0：

      进行光流跟踪操作执行cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

遍历所有特征点

4，     如果需要发布当前帧的特征点，则执行以下操作

     通过对极约束剔除outlier，执行rejectWithF()；

     设置mask，执行setMask()，假如合格特征点不够，利用mask均匀化提取新的特征点，执行cv::goodFeaturesToTrack()函数在下一帧图像中检测新的特征点

5，更新前后帧信息，去畸变同时计算特征点速度，执行undistortedPoints()

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;//名为 img 的 cv::Mat 对象来存储输入图像
    TicToc t_r;
    cur_time = _cur_time;

    if (EQUALIZE)
    {//如果开启了均衡化选项 (EQUALIZE)，则创建一个 CLAHE 对象，并应用均衡化操作到输入图像上，结果保存在 img 中
        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 = _img;

    if (forw_img.empty())
    {//检查是否存在前一帧图像 (forw_img)，如果不存在，则将当前帧图像 (_img) 赋值给前一帧、当前帧和下一帧图像
        prev_img = cur_img = forw_img = img;
    }
    else
    {//如果存在前一帧图像，则将当前帧图像赋值给下一帧图像
        forw_img = img;
    }
//清空下一帧特征点的向量 (forw_pts)
    forw_pts.clear();
//如果当前特征点的数量大于0，则进行光流跟踪操作
    if (cur_pts.size() > 0)
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;
        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]))
                status[i] = 0;
    //遍历所有特征点，如果特征点的状态为有效并且不在图像边界内，则将状态置为无效。
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(ids, status);
        reduceVector(cur_un_pts, status);
        reduceVector(track_cnt, status);
    //使用状态向量对先前的特征点、当前的特征点、下一帧的特征点、特征点的ID、无畸变的当前特征点和跟踪计数进行筛选
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
    }

    for (auto &n : track_cnt)
        n++;

    if (PUB_THIS_FRAME)
    {//如果需要发布当前帧的特征点，则执行以下操作：
        rejectWithF();//使用基础矩阵 F 进行外点剔除
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        setMask();//设置掩码，用于限制特征点检测的区域
        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<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;
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }//使用 goodFeaturesToTrack 函数在下一帧图像中检测新的特征点
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        ROS_DEBUG("add feature begins");
        TicToc t_a;
        addPoints();//将检测到的新特征点添加到特征跟踪器中
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }//更新先前帧、先前特征点、无畸变的先前特征点、当前帧、当前特征点和无畸变的当前特征点
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    cur_img = forw_img;
    cur_pts = forw_pts;
    undistortedPoints();//对当前特征点进行去畸变处理
    prev_time = cur_time;//更新先前帧的时间为当前时间
}

void FeatureTracker::rejectWithF()

void FeatureTracker::rejectWithF()
{
    if (forw_pts.size() >= 8)
    {//检查当前特征点数量是否大于等于8（用于计算基础矩阵），如果满足则开始运行 RANSAC 算法进行外点剔除。
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;
    //创建两个向量 un_cur_pts 和 un_forw_pts，用于存储无畸变的当前和下一帧特征点。
        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;
            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());
        //将无畸变后的点转换为像素坐标并保存到 un_cur_pts 中。
            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());
      //将下一帧特征点也进行无畸变化处理和像素坐标转换，并保存到 un_forw_pts 中
        }
/*使用 cv::findFundamentalMat 函数计算基础矩阵，使用 RANSAC 算法进行外点剔除，
得到状态向量 status，表示哪些特征点是内点（值为1）哪些是外点（值为0）。
*/
        vector<uchar> status;
        cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = cur_pts.size();
//基于状态向量，从先前帧、当前帧和下一帧的特征点向量、ID 向量、跟踪计数向量以及无畸变化后的当前帧特征点向量中移除外点
        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);
        //输出 RANSAC 算法的运行时间
        ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
    }
}

void FeatureTracker::setMask()

void FeatureTracker::setMask()
{//根据是否使用鱼眼相机，选择使用预定义的鱼眼相机掩膜还是创建一个全白的掩膜
    if(FISHEYE)//真，则使用 fisheye_mask 进行克隆
        mask = fisheye_mask.clone();
    else//否则创建一个与图像尺寸相同、像素值为255的 CV_8UC1 类型的掩膜
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
    

    // prefer to keep features that are tracked for long time
    //创建一个存储特征点计数、特征点坐标和特征点ID的向量 cnt_pts_id
    vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;
//遍历下一帧特征点，将每个特征点的计数、坐标和ID作为一个元组添加到 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])));
//使用自定义的比较函数对 cnt_pts_id 向量进行排序，按照特征点计数降序排列。
    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;
         });
//清空当前帧特征点、ID和计数的向量
    forw_pts.clear();
    ids.clear();
    track_cnt.clear();
//对于排序后的 cnt_pts_id 向量中的每个元素
    for (auto &it : cnt_pts_id)
    {
        if (mask.at<uchar>(it.second.first) == 255)// 如果该特征点在掩膜中的像素值为255（即未被其他特征点覆盖）
        {//将其特征点坐标、ID和计数添加到当前帧特征点、ID和计数的向量中
            forw_pts.push_back(it.second.first);
            ids.push_back(it.second.second);
            track_cnt.push_back(it.first);
            //并在掩膜中以特征点为中心、半径为 MIN_DIST 的范围内像素值设为0，表示该区域已被占用。
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }//完成特征点的筛选和掩膜更新。
}

void FeatureTracker::undistortedPoints()

void FeatureTracker::undistortedPoints()
{//清空当前帧无畸变特征点和特征点ID的向量 cur_un_pts 和 cur_un_pts_map
    cur_un_pts.clear();
    cur_un_pts_map.clear();
    //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
    for (unsigned int i = 0; i < cur_pts.size(); i++)
    {/*遍历当前帧所有畸变特征点，使用相机模型将其投影到三维空间中，并再次投影到无畸变图像平面上，将结果添加到 cur_un_pts 向量中。
    同时，将无畸变特征点和特征点ID作为一个键值对添加到 cur_un_pts_map 中。
    */
        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);
    }
    // caculate points velocity
    if (!prev_un_pts_map.empty())
    {//如果有上一帧的无畸变特征点信息
        double dt = cur_time - prev_time;//计算两帧之间的时间间隔dt
        pts_velocity.clear();
        for (unsigned int i = 0; i < cur_un_pts.size(); i++)
        {//遍历当前帧所有无畸变特征点，如果该特征点有对应的特征点ID
            if (ids[i] != -1)
            {//在上一帧的无畸变特征点中查找该特征点的ID，如果找到，则计算该特征点的速度，将结果添加到 pts_velocity 向量中。
                std::map<int, cv::Point2f>::iterator it;
                it = prev_un_pts_map.find(ids[i]);
                if (it != prev_un_pts_map.end())
                {
                    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));
                }//速度计算：将当前帧特征点和上一帧特征点在水平和垂直方向上的位移除以时间间隔 dt
                else//如果没有找到对应的上一帧特征点，则将该特征点的速度设置为0
                    pts_velocity.push_back(cv::Point2f(0, 0));
            }
            else
            {//如果该特征点没有对应的特征点ID，则将其速度设置为0。
                pts_velocity.push_back(cv::Point2f(0, 0));
            }
        }
    }
    else
    {//如果没有上一帧的无畸变特征点信息，则将所有特征点的速度设置为0。
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            pts_velocity.push_back(cv::Point2f(0, 0));
        }
    }//将当前帧无畸变特征点和特征点ID的信息保存在 prev_un_pts_map 中，以备下一帧使用
    prev_un_pts_map = cur_un_pts_map;
}