ESVIO代码详解

目录

ESVIO

整体逻辑:

代码底层框架源于vins，包含三个独立的ros功能包：feature_tracker, esvio_estimator和pose_graph。

功能包1：feature_tacker(前端特征提取与匹配)

包含event_detecor、feature_tracker、parameter、stereo_event_tracker_node、stereo_image_tracker_node五个核心文件。

包含两个节点：stereo_image_tracker和stereo_event_tracker

1.stereo_image_tracker_node的main函数

从main函数分析关键函数的调用链：

int main(int argc, char **argv)
{
    ros::init(argc, argv, "stereo_image_tracker");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    readParameters(n);

    // 初始化相机参数
    trackerData.stereo_readIntrinsicParameter(CAM_NAMES);

    // 订阅左右相机图像
    ros::Subscriber sub_img_left = n.subscribe(IMAGE_LEFT, 100, img_callback_left);
    ros::Subscriber sub_img_right = n.subscribe(IMAGE_RIGHT, 100, img_callback_right);

    // 发布特征点和可视化信息
    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);

    // 启动同步线程
    std::thread sync_thread{sync_process};
    ros::spin();
    return 0;
}

关键函数调用链：

2.stereo_event_tracker_node的main函数

基本同上

3.sync_process事件相机函数

值得学习的是对事件的处理方式

首先说明，对于事件队列：

这是左右相机事件的定义：queue<dvs_msgs::EventArray> events_left_buf;

队列（queue）：这是C++标准库中的一个容器适配器，实现了FIFO（先进先出）的数据结构。主要操作有：

push：添加元素到队尾
pop：移除队首元素
front：访问队首元素
empty：检查队列是否为空

dvs_msgs::EventArray：这是一个ROS消息类型，用于存储事件相机捕获的事件数据。从你提供的头文件可以看出它包含以下字段：

header：标准ROS消息头，包含时间戳和坐标帧ID
height：图像高度（行数）
width：图像宽度（列数）
events：事件数组，类型为Event[]

Event 结构：根据定义，每个Event包含：

x：事件在图像上的x坐标（列）
y：事件在图像上的y坐标（行）
ts：事件的时间戳
polarity：事件的极性（true或false，表示亮度增加或减少）

所以，queue<dvs_msgs::EventArray> events_left_buf 是一个队列，用于存储来自左相机的事件数据包。每个数据包（EventArray）包含在特定时间窗口内捕获的一系列事件。

在sync_process函数中，每次循环只处理队列中的一个dvs_msgs::EventArray消息

void sync_process()
{
    while(1)
    {
            dvs_msgs::EventArray event_left, event_right;
            std_msgs::Header header;
            double msg_timestamp = 0.0;

            m_buf_event.lock(); // 锁定事件缓冲区互斥量，防止在访问缓冲区时发生竞争条件

            // 检查左右事件缓冲区是否都不为空
            if (!events_left_buf.empty() && !events_right_buf.empty()){
                // 获取左右事件队列前端的时间戳
                double time_left = events_left_buf.front().header.stamp.toSec();
                double time_right = events_right_buf.front().header.stamp.toSec();
                // 使用左相机事件的时间戳作为消息时间戳
                msg_timestamp = time_left;

                // 如果左相机事件时间戳比右相机早0.2秒以上，则丢弃左相机事件
                if(time_left < time_right - 0.2) //tolerance
                {
                    events_left_buf.pop();
                    printf("throw events1\n");
                }
                // 如果左相机事件时间戳比右相机晚0.2秒以上，则丢弃右相机事件
                else if(time_left > time_right + 0.2)
                {
                    events_right_buf.pop();
                    printf("throw events2\n");
                }
                // 如果两个事件的时间戳差距在0.2秒内，则认为它们是同步的
                else
                {
                    // 记录消息时间戳和头部信息
                    msg_timestamp = events_left_buf.front().header.stamp.toSec();
                    header = events_left_buf.front().header;
                    // 获取左相机事件
                    event_left = events_left_buf.front();
                    // 移除处理过的左相机事件
                    events_left_buf.pop();
                    // 获取右相机事件
                    event_right = events_right_buf.front();
                    // 移除处理过的右相机事件
                    events_right_buf.pop();
                }

            }
            // 解锁互斥量
            m_buf_event.unlock();

            if(event_left.events.size()!= 0){

                handle_stereo_event(event_left, event_right, msg_timestamp);

            }

        std::chrono::milliseconds dura(2);
        std::this_thread::sleep_for(dura);
    }
    

}

4.handle_stereo_event函数

在trackerData.trackEvent()函数中，完成左右相机特征点的检测和匹配，匹配的结果保存在trackerData对象中，包括：

trackerData.ids: 左相机特征点的ID
trackerData.ids_right: 右相机特征点的ID
以及相应的位置、速度等信息

当同一个特征点在左右相机中都被检测到时，它们被赋予相同的特征ID，在hash_ids集合中保存了左相机所有特征点的ID，通过检查右相机特征点的ID是否在hash_ids中存在，可以确定这个特征点是否在两个相机中都被检测到

具体实现逻辑

void handle_stereo_event(const dvs_msgs::EventArray &event_left, const dvs_msgs::EventArray &event_right, double msg_timestamp)
{   
    static int cnt = 0;
    const int n_event = event_left.events.size();

    if(n_event == 0){
        ROS_WARN("not event, please move the event camera or check whether connecting");
        return;
    }

    if(first_image_flag)// 处理第一帧事件的特殊情况
    {
        first_image_flag = false;
        first_image_time = msg_timestamp;
        last_image_time = msg_timestamp;
        return;
    }

    // 检测事件流是否连续，如果时间戳异常则重置跟踪器
    if (msg_timestamp - last_image_time > 1.0 || msg_timestamp < last_image_time)
    {
        ROS_WARN("event stream discontinue! reset the event 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);// reset para and reset operation
        return;
    }
    last_image_time = msg_timestamp;

    // frequency control
    if (round(1.0 * pub_count / (msg_timestamp - first_image_time)) <= FREQ) // ?这里的msg_timestamp 还有first_image_time的赋值逻辑
    {
        PUB_THIS_FRAME = true;// 标记当前帧需要发布

        // 当达到目标频率时重置计时
        if (abs(1.0 * pub_count / (msg_timestamp - first_image_time) - FREQ) < 0.01 * FREQ)
        {
            first_image_time = msg_timestamp;
            pub_count = 0;
        }
    }
    else
        PUB_THIS_FRAME = false;

    double msg_timestamp_left = event_left.events.back().ts.toSec();

    if (Do_motion_correction == 0){
        // 不进行运动校正，直接跟踪事件
        trackerData.trackEvent(msg_timestamp_left, event_left, event_right);//optiflow track 核心功能4 匹配
    }else{
        Motion_correction_value motion_compensation;// 进行运动校正，需要计算运动补偿参数
        // 获取事件批次的起始和结束时间
        double t_left_0 = event_left.events[0].ts.toSec();  // 第一个事件的时间戳
        double t_left_1 = event_left.header.stamp.toSec();  // 事件批次的时间戳
        
        // 声明用于存储动态信息的变量
        Eigen::Vector3d temp_v;  // 当前速度
        Eigen::Vector3f temp_a;  // 当前加速度
        double temp_time;        // 当前时间
        Eigen::Vector4d State_;  // 保存当前状态和时间
        Eigen::Vector2d t_0_1(t_left_0,t_left_1);  // 事件时间窗口
        Eigen::Vector3f omega_avg_;  // IMU角速度
        Eigen::Vector3f accel_avg_;  // IMU加速度

        // 如果有IMU数据
        if (!imu_buf.empty()){
            // 如果有里程计数据，获取当前速度
            if(!odom_buffer_.empty()){
                temp_time = odom_buffer_.front()->header.stamp.toSec();
                // 获取当前无人机速度
                temp_v[0]=odom_buffer_.front()->twist.twist.linear.x;
                temp_v[1]=odom_buffer_.front()->twist.twist.linear.y;
                temp_v[2]=odom_buffer_.front()->twist.twist.linear.z;
                odom_buffer_.pop();  // 弹出处理过的里程计数据

                // 保存当前状态
                State_[0]=temp_v[0];
                State_[1]=temp_v[1];
                State_[2]=temp_v[2];
                State_[3]=temp_time;

                // 更新前一时刻和当前时刻的速度
                v_pre[0] = v_cur[0];
                v_pre[1] = v_cur[1];
                v_pre[2] = v_cur[2];
                v_cur[0] = temp_v[0];
                v_cur[1] = temp_v[1];
                v_cur[2] = temp_v[2];

                // 更新前一时刻和当前时刻的时间
                t_pre = t_cur;  //t_pre存储上一次获取里程计数据的时间戳
                t_cur = temp_time; //t_cur存储当前获取的里程计数据的时间戳

                // 计算加速度（速度差除以时间差）
                temp_a[0] = (v_cur[0] - v_pre[0])/(t_cur - t_pre);  //_cur - t_pre是连续两次获取里程计数据之间的时间差
                temp_a[1] = (v_cur[1] - v_pre[1])/(t_cur - t_pre);
                temp_a[2] = (v_cur[2] - v_pre[2])/(t_cur - t_pre);
            }

            // 移除事件时间窗口之前的IMU数据
            while(!imu_buf.empty()){
                if (imu_buf.front()->header.stamp.toSec() < t_left_0)
                    imu_buf.pop();
                else
                    break;
            }

            // 获取IMU数据（角速度和线性加速度）
            if (!imu_buf.empty()){
                omega_avg_[0]=imu_buf.front()->angular_velocity.x;
                omega_avg_[1]=imu_buf.front()->angular_velocity.y;
                omega_avg_[2]=imu_buf.front()->angular_velocity.z;
                accel_avg_[0]=imu_buf.front()->linear_acceleration.x;
                accel_avg_[1]=imu_buf.front()->linear_acceleration.y;
                accel_avg_[2]=imu_buf.front()->linear_acceleration.z-9.805;  // 减去重力加速度
            }
        }
        
        // 构建运动补偿数据结构 ？实现方法
        motion_compensation = std::make_pair(is_nolinear,std::make_pair(std::make_pair (State_,v_pre),std::make_pair(t_0_1,std::make_pair(temp_a,omega_avg_))));

        // 使用运动补偿参数跟踪事件
        trackerData.trackEvent(msg_timestamp_left, event_left, event_right, motion_compensation);
    }

    if (SHOW_TRACK)//show the tracking process
        {
            cv::Mat imageTrack=trackerData.getTrackImage();
            cv::Mat imgTrack_two =trackerData.getTrackImage_two();
            cv::Mat imgTrack_two_point =trackerData.getTrackImage_two_point();
            cv::Mat Time_surface_map =trackerData.gettimesurface();
            cv::Mat event_loop = trackerData.getEventloop();
            pubTrackImage(imageTrack,imgTrack_two,imgTrack_two_point,Time_surface_map,last_image_time,event_loop);
        }


   // 如果需要发布当前帧的特征点 创建和填充了一个ROS点云消息，用于传输特征点信息
    if (PUB_THIS_FRAME)
    {
        pub_count++;  // 更新发布计数

        {
            // 创建点云消息，用于存储特征点 智能指针，指向新分配的PointCloud消息对象 PointCloud是ROS中传输3D点集合的标准消息类型
            sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
            // 创建特征点的各种属性通道
            sensor_msgs::ChannelFloat32 id_of_point;         // 特征点ID
            sensor_msgs::ChannelFloat32 u_of_point;          // 特征点u坐标
            sensor_msgs::ChannelFloat32 v_of_point;          // 特征点v坐标
            sensor_msgs::ChannelFloat32 velocity_x_of_point; // 特征点x方向速度
            sensor_msgs::ChannelFloat32 velocity_y_of_point; // 特征点y方向速度

            // 设置点云消息的时间戳和坐标系
            feature_points->header.stamp = ros::Time(msg_timestamp);
            feature_points->header.frame_id = "world";

            int camera_id = 0;  // 左相机ID为0
            int feature_id;
            geometry_msgs::Point32 p;
            
            // 处理左图像上的特征点
            set<int> hash_ids;  // 用于跟踪已处理的特征点ID
            for(size_t j=0; j<trackerData.ids.size(); j++){
               // 只处理被跟踪超过1帧的特征点
               if (trackerData.track_cnt[j] > 1)
                {
                    feature_id = trackerData.ids[j];
                    // 归一化平面上的坐标
                    p.x = trackerData.cur_un_pts[j].x;
                    p.y = trackerData.cur_un_pts[j].y;
                    p.z = 1;
                    
                    // 记录已处理的特征点ID
                    hash_ids.insert(feature_id);
                    // 添加特征点到点云
                    feature_points->points.push_back(p);
                    // 添加特征点属性
                    id_of_point.values.push_back(feature_id * NUM_OF_CAM_stereo + camera_id);
                    u_of_point.values.push_back(trackerData.cur_pts[j].x);
                    v_of_point.values.push_back(trackerData.cur_pts[j].y);
                    velocity_x_of_point.values.push_back(trackerData.pts_velocity[j].x);
                    velocity_y_of_point.values.push_back(trackerData.pts_velocity[j].y);
                } 
            }
            
            // 处理右图像上的特征点
            camera_id = 1;  // 右相机ID为1
            for(size_t j=0; j<trackerData.ids_right.size(); j++){
               feature_id = trackerData.ids_right[j];
               // 只处理在左图像中也被跟踪的特征点（双目匹配点）trackEvent函数已经完成了匹配
               if (hash_ids.find(feature_id) != hash_ids.end())
                {
                    // 归一化平面上的坐标
                    p.x = trackerData.cur_un_right_pts[j].x;
                    p.y = trackerData.cur_un_right_pts[j].y;
                    p.z = 1;
                    // 添加特征点到点云
                    feature_points->points.push_back(p);
                    // 添加特征点属性
                    id_of_point.values.push_back(feature_id * NUM_OF_CAM_stereo + camera_id);
                    u_of_point.values.push_back(trackerData.cur_right_pts[j].x);
                    v_of_point.values.push_back(trackerData.cur_right_pts[j].y);
                    velocity_x_of_point.values.push_back(trackerData.right_pts_velocity[j].x);
                    velocity_y_of_point.values.push_back(trackerData.right_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);

            // 跳过第一帧（因为第一帧没有光流速度）
            if (!init_pub)
            {
                init_pub = 1;
            }
            else
                // 发布特征点消息到姿态图优化节点
                pub_img.publish(feature_points);
        }
    }
}

5.FeatureTracker::trackImage函数

trackImage函数接收当前时间戳和左右相机图像，跟踪特征点的运动，并在需要时检测新的特征点。它实现了：

时间上的特征跟踪（前后帧）：使用LK光流法跟踪特征点从前一帧到当前帧的运动,可选的反向光流检查提高跟踪可靠性,移除边界外和跟踪失败的点

特征点检测：当特征点数量不足时，在合适区域检测新特征点, 使用掩码确保新特征点与现有点保持足够距离, 为新特征点分配唯一ID和初始跟踪计数

空间上的特征匹配（左右相机）：使用光流法在右图像中寻找左图像特征点的对应点, 通过反向光流检查验证匹配的可靠性, 共享特征点ID（ids_right = ids）实现左右图像特征点的关联

特征点处理：计算归一化坐标（去除镜头畸变）;计算特征点速度（光流速度）;维护特征点的跟踪计数和映射表

整个函数实现了一个完整的双目特征跟踪系统，既能在时间上跟踪特征点的运动，又能在空间上实现左右相机的特征匹配

一些非常重要的细节：

cv::goodFeaturesToTrack()函数: OpenCV中实现Shi-Tomasi角点检测器的函数。

Shi-Tomasi角点检测器的使用逻辑与光流跟踪的顺序: 不是所有帧都检测新角点：只有当PUB_THIS_FRAME为真（基于频率控制）且现有特征点数量不足（小于MAX_CNT_IMG）时，才会检测新角点。已跟踪的特征点优先：系统优先使用光流法跟踪前一帧的特征点到当前帧，仅在现有特征点数量不足时，才在当前帧中检测新角点。

混合使用策略：当前帧的特征点包括两部分：从前一帧跟踪过来的旧特征点和在当前帧新检测的特征点。

void FeatureTracker::trackImage(double _cur_time, const cv::Mat &img_left, const cv::Mat &img_right)
{   
    TicToc t_r; // 计时器，用于测量函数执行时间

    cur_time = _cur_time; // 保存当前时间戳到类成员变量

    int row = img_left.rows;
    int col = img_left.cols;

    cv::Mat rightImg = img_right;

    // 如果当前图像为空（首次调用），初始化前一帧和当前帧为传入的左图像
    if(cur_img_left.empty()){
        prev_img_left = cur_img_left = img_left; 
    }else{
        cur_img_left = img_left;  // 否则，更新当前帧
    }

    cur_pts.clear(); // 清空当前特征点容器，准备存储新的跟踪结果
    
    if (prev_pts.size() > 0) // 如果前一帧有特征点，则进行特征跟踪
    {
        TicToc t_o; // 计时器，用于测量光流计算时间

        // 状态向量和误差向量，用于存储光流跟踪结果
        vector<uchar> status;
        vector<float> err;
        
        // 使用金字塔LK光流法跟踪前一帧到当前帧的特征点
        // 参数：前一帧图像，当前帧图像，前一帧特征点，当前帧特征点（输出），状态标志，误差，窗口大小，金字塔层数
        cv::calcOpticalFlowPyrLK(prev_img_left, cur_img_left, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);

        // 如果启用了反向光流检查（提高跟踪可靠性）
        if(FLOW_BACK)
        {
            // 反向状态向量和反向特征点（初始化为前一帧特征点）
            vector<uchar> reverse_status;
            vector<cv::Point2f> reverse_pts = prev_pts;
            
            // 计算反向光流，从当前帧返回前一帧
            cv::calcOpticalFlowPyrLK(cur_img_left, prev_img_left, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 3);
            
            // 遍历所有特征点，验证正反向光流的一致性
            for(size_t i = 0; i < status.size(); i++)
            {
                // 如果正向跟踪成功、反向跟踪成功、且反向跟踪回到的位置与原位置距离小于0.5像素
                if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
                {
                    // 保持该点的状态为有效
                    status[i] = 1;
                }
                else
                    // 否则标记为无效
                    status[i] = 0;
            }
        }
        
        // 检查特征点是否在图像边界内
        for (int i = 0; i < int(cur_pts.size()); i++)
            if (status[i] && !inBorder(cur_pts[i]))
                status[i] = 0;  // 如果超出边界，标记为无效
        
        // 根据状态标志移除无效的特征点及其相关信息
        reduceVector(prev_pts, status);  // 移除无效的前一帧特征点
        reduceVector(cur_pts, status);   // 移除无效的当前帧特征点
        reduceVector(ids, status);       // 移除无效的特征点ID
        reduceVector(track_cnt, status); // 移除无效的跟踪计数
    }

    for (auto &n : track_cnt) // 对所有成功跟踪的特征点，增加其跟踪计数
        n++;
    
    if (PUB_THIS_FRAME) // 如果当前帧需要发布（标志位置位在handle_stereo_image函数频率控制部分中）
    {        
        TicToc t_m; // 计时器，用于测量掩码设置时间
        Image_setMask(); //核心功能 设置掩码，避免新特征点与现有点太近

        TicToc t_t; // 计时器，用于测量特征检测时间

        // 计算需要检测的新特征点数量，确保总数不超过最大值
        int n_max_cnt = MAX_CNT_IMG - static_cast<int>(cur_pts.size());

        // 如果需要检测新特征点
        if (n_max_cnt > 0)
        {
            // 检查掩码是否准备好
            if(mask_image.empty())
                cout << "mask is empty " << endl;
            if (mask_image.type() != CV_8UC1)
                cout << "mask type wrong " << endl;

            // 使用Shi-Tomasi角点检测器在掩码允许的区域检测新特征点
            // 参数：输入图像，输出点，最大点数，质量阈值，最小距离，掩码    
            cv::goodFeaturesToTrack(cur_img_left, n_pts, MAX_CNT_IMG - cur_pts.size(), 0.01, MIN_DIST_IMG, mask_image);

        }
        else
            n_pts.clear(); // 如果不需要新特征点，清空容器
         
        for (auto &p : n_pts) // 为每个新检测的特征点分配ID和初始跟踪计数
        {
            cur_pts.push_back(p);       // 添加到当前特征点列表
            ids.push_back(n_id++);      // 分配唯一ID并递增ID计数器
            track_cnt.push_back(1);     // 初始化跟踪计数为1
        }
    }
    
    cur_un_pts.clear(); // 清空归一化特征点容器
    cur_un_pts = undistortedPts(cur_pts, stereo_m_camera[0]); // 计算特征点的归一化坐标（去畸变）todo
    pts_velocity.clear(); // 清空特征点速度容器
    pts_velocity = ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map); // 计算特征点的速度（基于前后帧的位置变化）todo ？计算的是一整个序列的速度吗

    // 如果右图像不为空，处理双目特征匹配
    if(!img_right.empty())
    {
        ids_right.clear();
        cur_right_pts.clear();
        cur_un_right_pts.clear();
        right_pts_velocity.clear();
        cur_un_right_pts_map.clear();

        // 如果左图像有特征点
        if(!cur_pts.empty())
        {
            // 用于存储反向检查的点
            vector<cv::Point2f> reverseLeftPts;
            
            // 状态向量和误差向量
            vector<uchar> status, statusRightLeft;
            vector<float> err;
            
            // 使用光流法在右图像中寻找左图像特征点的对应点
            // 参数：左图像，右图像，左图像特征点，右图像特征点（输出），状态标志，误差，窗口大小，金字塔层数
            cv::calcOpticalFlowPyrLK(cur_img_left, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
            
            // 如果启用反向检查且右图像特征点不为空
            if(FLOW_BACK && !cur_right_pts.empty())
            {
                // 计算从右图像到左图像的反向光流
                cv::calcOpticalFlowPyrLK(rightImg, cur_img_left, cur_right_pts, reverseLeftPts, statusRightLeft, err, cv::Size(21, 21), 3);
                
                // 遍历所有特征点，验证正反向光流的一致性
                for(size_t i = 0; i < status.size(); i++)
                {
                    // 如果正向匹配成功、反向匹配成功、右图点在边界内、且反向匹配回到的位置与原位置距离小于0.5像素
                    if(status[i] && statusRightLeft[i] && inBorder(cur_right_pts[i]) && distance(cur_pts[i], reverseLeftPts[i]) <= 0.5)
                    {
                        // 保持该点的状态为有效
                        status[i] = 1;
                    }
                    else
                        // 否则标记为无效
                        status[i] = 0;
                }
            }

            // //fundamental matrix check
            
                // vector<cv::Point2f> un_l_pts, un_r_pts; 
                // un_l_pts.reserve(ids.size()); 
                // un_r_pts.reserve(ids.size()); 

                // for(int i=0; i<ids.size(); i++){
                //     if(status[i]){
                //         Eigen::Vector3d tmp_p;
                //         stereo_m_camera[0]->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
                //         // ouf2 << i<<" l: "<< tmp_p.transpose()<<" "; 
                //         // 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_l_pts.push_back(cv::Point2f(tmp_p.x(), tmp_p.y())); 
                //         // ouf2<<" proj: "<<tmp_p.x()<<" "<<tmp_p.y()<<" "<<endl; 

                //         stereo_m_camera[1]->liftProjective(Eigen::Vector2d(cur_right_pts[i].x, cur_right_pts[i].y), tmp_p);
                //         // ouf2 << i<<" r: "<< tmp_p.transpose()<<" "; 
                //         // 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_r_pts.push_back(cv::Point2f(tmp_p.x(), tmp_p.y()));
                //         // ouf2<<" proj: "<<tmp_p.x()<<" "<<tmp_p.y()<<" "<<endl; 
                //         // ouf<<i<<" "<<cur_pts[i].x<<" "<<cur_pts[i].y<<" "<<cur_right_pts[i].x<<" "<<cur_right_pts[i].y<<endl;
                //     }
                // }

            // // use Tlr to to verification
            
            // 关键步骤：右图像特征点继承左图像对应点的ID
            // 这确保了左右图像中匹配的特征点共享相同的ID
            ids_right = ids;

            // 根据状态标志移除无效的右图像特征点及其相关信息
            reduceVector(cur_right_pts, status);  // 移除无效的右图像特征点
            reduceVector(ids_right, status);      // 移除无效的右图像特征点ID
            reduceVector(track_cnt_right, status); // 移除无效的右图像特征点跟踪计数

            // 对所有成功匹配的右图像特征点，增加其跟踪计数
            for (auto &n : track_cnt_right)
                n++;

            // 为每个右图像特征点初始化跟踪计数
            // 注意：这里可能有逻辑问题，因为已经增加了现有点的计数，再为所有点添加计数=1可能不合理
            for (auto &p : cur_right_pts)
            {
                track_cnt_right.push_back(1);
            }

            // 计算右图像特征点的归一化坐标（去畸变）
            cur_un_right_pts = undistortedPts(cur_right_pts, stereo_m_camera[1]);
            
            // 计算右图像特征点的速度
            right_pts_velocity = ptsVelocity(ids_right, cur_un_right_pts, cur_un_right_pts_map, prev_un_right_pts_map);
        }
        
        // 更新前一帧右图像的归一化点映射
        prev_un_right_pts_map = cur_un_right_pts_map;
    }

    // 如果需要显示跟踪结果
    if(SHOW_TRACK)
        // 绘制跟踪和匹配的特征点
        drawTrack(cur_img_left, rightImg, ids, cur_pts, cur_right_pts, prevLeftPtsMap);

    // 更新前一帧的图像、特征点和时间戳，为下一次跟踪做准备
    prev_img_left = cur_img_left;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    prev_un_pts_map = cur_un_pts_map;
    prev_time = cur_time;

    // 更新前一帧左图像的特征点ID映射表（ID到坐标的映射）
    prevLeftPtsMap.clear();
    for(size_t i = 0; i < cur_pts.size(); i++)
        prevLeftPtsMap[ids[i]] = cur_pts[i];
    
    return; // 函数结束
}

6.FeatureTracker::trackEvent函数（非运动补偿）

函数形式：void FeatureTracker::trackEvent(double _cur_time, const dvs_msgs::EventArray &event_left, const dvs_msgs::EventArray &event_right)

与图像跟踪不同的一点在于使用自定义的Event_FeaturesToTrack函数补充特征点，这个将在后面详细介绍

几个小问题：

  1. 什么是当前帧左图像特征点的ID映射表（ID到坐标的映射）？

    ID映射表类型：map<int, cv::Point2f> curLeftPtsMap;

    ID映射表是一个数据结构，将特征点的唯一标识符(ID)映射到其在图像中的坐标位置。

  2. 什么是特征点的归一化坐标（去畸变）？形式是什么样的？

    归一化坐标是指将图像中的特征点坐标转换为去除镜头畸变后的标准化坐标。

    在代码中，这是通过undistortedPts函数实现的：

  3. 两帧之间匹配的特征点对是以什么形式返回的？

    在这个实现中，两帧之间匹配的特征点对不是作为单独的数据结构直接返回的，而是通过几个平行数组隐式表示。

    核心数据结构：

     prev_pts和cur_pts：前一帧和当前帧中对应特征点的坐标数组
 ids：特征点的唯一标识符数组
 status：表示每对特征点匹配是否有效的布尔数组

    匹配形式：

     相同索引对应关系：prev_pts[i]与cur_pts[i]构成一对匹配的特征点，它们共享相同的ID ids[i]
 数组大小一致：经过reduceVector函数处理后，这些数组大小相同，且只包含有效匹配

在handle_stereo_event函数中：

if (Do_motion_correction == 0)
// 不进行运动校正，直接跟踪事件
trackerData.trackEvent(msg_timestamp_left, event_left, event_right);

这里调用了trackEvent进行匹配，处理后的匹配结果主要保存在 FeatureTracker类的多个成员变量中，这些变量在函数执行过程中被更新。主要包括：

  1. 特征点坐标和关联信息

    cur_pts: 当前左图像中的特征点坐标

    cur_right_pts: 当前右图像中的特征点坐标

    ids: 左图像特征点的唯一ID

    ids_right: 右图像特征点的唯一ID（与左图对应）

    track_cnt: 每个特征点被连续跟踪的帧数

    track_cnt_right: 右图像特征点被连续跟踪的帧数

  2. 归一化坐标和速度信息

    cur_un_pts: 当前左图像特征点的归一化坐标（去畸变）

    cur_un_right_pts: 当前右图像特征点的归一化坐标

    pts_velocity: 左图像特征点的速度

    right_pts_velocity: 右图像特征点的速度

  3. 映射关系

    curLeftPtsMap: 当前左图像特征点ID到坐标的映射

    curRightPtsMap: 当前右图像特征点ID到坐标的映射

    prevLeftPtsMap: 保存当前左图像特征点信息，下一帧将作为前一帧的参考

在stereo_event_tracker_node.cpp中开头定义了全局变量trackerData, 函数可以直接读取trackerData的成员变量，因此跟踪函数调用以后能够进一步得到匹配的特征点对。

void FeatureTracker::trackEvent(double _cur_time, const dvs_msgs::EventArray &event_left, const dvs_msgs::EventArray &event_right)
{
    // 保存当前时间戳到类成员变量
    cur_time = _cur_time;
    
    // 声明用于存储时间面图像的变量
    cv::Mat img_left;
    cv::Mat img_right;
    cv::Mat rightImg; // 用于绘制跟踪结果的右图像

    // 如果检测器尚未初始化，进行初始化
    // FLAG_DETECTOR_NOSTART是一个标志，表示检测器是否已启动
    if(FLAG_DETECTOR_NOSTART){
        FLAG_DETECTOR_NOSTART = false;
        // 使用事件图像的列数和行数初始化检测器  核心，这里与event_detector.cpp关联起来了
        detector.init(COL_event, ROW_event);
    }

    // 创建左右相机的事件矩阵（Surface of Active Events，SAE）
    // 初始化为全零的彩色图像
    detector.cur_event_mat_left = cv::Mat::zeros(cv::Size(COL_event, ROW_event), CV_8UC3);
    detector.cur_event_mat_right = cv::Mat::zeros(cv::Size(COL_event, ROW_event), CV_8UC3);
    
    TicToc t_SAE;// 计时器，用于测量SAE创建的时间

    // 遍历左相机的所有事件，更新左相机的SAE
    for (const dvs_msgs::Event& e_left:event_left.events){
        // 调用检测器的createSAE_left方法，传入事件的时间戳、坐标和极性
        detector.createSAE_left(e_left.ts.toSec(), e_left.x, e_left.y, e_left.polarity);
    }

    // 遍历右相机的所有事件，更新右相机的SAE
    for (const dvs_msgs::Event& e_right:event_right.events){
        detector.createSAE_right(e_right.ts.toSec(), e_right.x, e_right.y, e_right.polarity);
    }

    // 获取当前左右相机的事件矩阵（SAE）
    cv::Mat event_mat_left = detector.cur_event_mat_left;
    cv::Mat event_mat_right = detector.cur_event_mat_right;

    // 将SAE转换为时间面（Time Surface）
    // 时间面是表示每个像素最近事件发生时间的图像
    const cv::Mat time_surface_map_left = detector.SAEtoTimeSurface_left(cur_time);
    const cv::Mat time_surface_map_right = detector.SAEtoTimeSurface_right(cur_time);

    // 复制时间面用于可视化
    time_surface_visualization_left = time_surface_map_left.clone();
    time_surface_visualization_right = time_surface_map_right.clone();
    
    // 复制时间面用于特征跟踪
    cv::Mat time_surface_left = time_surface_map_left.clone();
    cv::Mat time_surface_right = time_surface_map_right.clone();
    
    // 如果启用了均衡化处理
    if (EQUALIZE)
    {
        // 创建对比度受限的自适应直方图均衡化（CLAHE）对象
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
        
        // 对左右时间面应用CLAHE
        clahe->apply(time_surface_left, img_left);
        clahe->apply(time_surface_right, img_right);
        
        // 将结果归一化到0-255范围
        cv::normalize(img_left, img_left, 0, 255, CV_MINMAX);
        cv::normalize(img_right, img_right, 0, 255, CV_MINMAX);
    }
    else
    {
        // 如果不均衡化，直接使用原始时间面
        img_left = time_surface_left;
        img_right = time_surface_right;
    }


    // 如果当前左图像为空（首次调用），初始化前一帧和当前帧
    if(cur_img_left.empty()){
        prev_img_left = cur_img_left = img_left; 
        prev_event_mat_left = event_mat_left; 
    }else{
        // 否则，更新当前左图像
        cur_img_left = img_left; 
    }
    
    // 清空当前左图像特征点容器
    cur_pts.clear();

    // 如果当前右图像为空（首次调用），初始化前一帧和当前帧
    if(cur_img_right.empty()){
        prev_img_right = cur_img_right = img_right;
    }else{
        // 否则，更新当前右图像
        cur_img_right = img_right;
    }
    
    // 清空当前右图像特征点容器
    cur_right_pts.clear();
    
    if (prev_pts.size() > 0) // 如果前一帧有特征点，则进行特征跟踪
    {   
        // 计时器，用于测量光流计算时间
        TicToc t_o;
        
        // 状态向量和误差向量，用于存储光流跟踪结果
        vector<uchar> status;
        vector<float> err;
        // 使用金字塔LK光流法跟踪前一帧到当前帧的特征点
        cv::calcOpticalFlowPyrLK(prev_img_left, cur_img_left, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);

        // 如果启用了反向光流检查
        if(FLOW_BACK)
        {
            vector<uchar> reverse_status;
            vector<cv::Point2f> reverse_pts = prev_pts;
            
            // 计算反向光流，从当前帧返回前一帧
            // 注意：这里使用了更多参数，包括终止条件和初始流使用标志
            cv::calcOpticalFlowPyrLK(cur_img_left, prev_img_left, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1, 
            cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
            
            // 遍历所有特征点，验证正反向光流的一致性
            for(size_t i = 0; i < status.size(); i++)
            {
                // 如果正向跟踪成功、反向跟踪成功、且反向跟踪回到的位置与原位置距离小于0.5像素
                if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
                {
                    // 保持该点的状态为有效
                    status[i] = 1;
                }
                else
                    // 否则标记为无效
                    status[i] = 0;
            }
        }
        
        // 检查特征点是否在事件图像边界内
        for (int i = 0; i < int(cur_pts.size()); i++)
            if (status[i] && !inBorder_event(cur_pts[i]))
                status[i] = 0;  // 如果超出边界，标记为无效
        
        // 根据状态标志移除无效的特征点及其相关信息
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
    }

    for (auto &n : track_cnt) // 对所有成功跟踪的特征点，增加其跟踪计数
        n++;

    // 如果当前帧需要发布
    if (PUB_THIS_FRAME)
    {
        // 使用基本矩阵RANSAC拒绝外点
        rejectWithF_event();//核心 todo
        
        // 计时器，用于测量掩码设置时间
        TicToc t_m;
        
        // 设置事件图像的掩码，避免新特征点与现有点太近
        Event_setMask(); //核心 todo

        // 计算需要检测的新特征点数量
        int n_max_cnt = MAX_CNT - static_cast<int>(cur_pts.size());

        // 如果需要检测新特征点
        if (n_max_cnt > 0)
        {   
            // 检查掩码是否准备好
            if(mask_event.empty())
                cout << "the time surface mask is empty " << endl;
            if (mask_event.type() != CV_64FC1)
                cout << "time surface mask type wrong " << endl;
            if (mask_event.size() != time_surface_left.size())
                cout << "wrong size" << endl;
            
            // 在事件数据中检测新特征点
            // 与传统图像不同，这里使用自定义的Event_FeaturesToTrack函数 核心 todo
            Event_FeaturesToTrack(event_left, n_pts, cur_pts, MAX_CNT - cur_pts.size(), MIN_DIST, mask_event, time_surface_left);
        }
        else
            // 如果不需要新特征点，清空容器
            n_pts.clear();

        // 为每个新检测的特征点分配ID和初始跟踪计数
        for (auto &p : n_pts)
        {
            cur_pts.push_back(p);
            ids.push_back(n_id++);
            track_cnt.push_back(1);
        }
    }

    // 清空当前归一化特征点容器
    cur_un_pts.clear();
    
    // 计算特征点的归一化坐标（去畸变）
    cur_un_pts = undistortedPts(cur_pts, stereo_m_camera[0]);  //核心 todo
    
    // 清空特征点速度容器
    pts_velocity.clear();
    
    // 计算特征点的速度
    pts_velocity = ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map);  //核心 todo

    // 如果右图像不为空，处理双目特征匹配
    if(!img_right.empty())
    {
        // 清空右图像相关的容器
        ids_right.clear();
        cur_right_pts.clear();
        cur_un_right_pts.clear();
        right_pts_velocity.clear();
        cur_un_right_pts_map.clear();
        track_cnt_right.clear();

        if(!cur_pts.empty())// 如果左图像有特征点
        {
            vector<cv::Point2f> reverseLeftPts;// 用于存储反向检查的点

            // 状态向量和误差向量
            vector<uchar> status, statusRightLeft;
            vector<float> err;
            // 使用光流法在右图像中寻找左图像特征点的对应点
            cv::calcOpticalFlowPyrLK(cur_img_left, cur_img_right, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
      
            // // reverse check cur right ---- cur left matching
            if(FLOW_BACK && !cur_right_pts.empty()) // 如果启用反向光流检查且右图像特征点不为空
            {
                // 计算从右图像到左图像的反向光流
                cv::calcOpticalFlowPyrLK(cur_img_right, cur_img_left, cur_right_pts, reverseLeftPts, statusRightLeft, err, cv::Size(21, 21), 3);
                
                // 遍历所有特征点，验证正反向光流的一致性
                for(size_t i = 0; i < status.size(); i++)
                {
                    // 如果正向匹配成功、反向匹配成功、右图点在边界内、且反向匹配回到的位置与原位置距离小于0.5像素
                    if(status[i] && statusRightLeft[i] && inBorder_event(cur_right_pts[i]) && distance(cur_pts[i], reverseLeftPts[i]) <= 0.5)
                    {
                        status[i] = 1;// 保持该点的状态为有效
                    }
                    else
                        status[i] = 0;// 否则标记为无效
                }
            }

            ids_right = ids;// 右图像特征点继承左图像对应点的ID

            // 根据状态标志移除无效的右图像特征点及其相关信息
            reduceVector(cur_right_pts, status);
            reduceVector(ids_right, status);
            reduceVector(track_cnt_right, status);

            // // fundamental matrix check // // 
            
            //     vector<cv::Point2f> un_l_pts, un_r_pts; 
            //     un_l_pts.reserve(ids.size()); 
            //     un_r_pts.reserve(ids.size()); 

            //     for(int i=0; i<ids.size(); i++){
            //         if(status[i]){
            //             Eigen::Vector3d tmp_p;
            //             stereo_m_camera[0]->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);

            //             un_l_pts.push_back(cv::Point2f(tmp_p.x(), tmp_p.y())); 


            //             stereo_m_camera[1]->liftProjective(Eigen::Vector2d(cur_right_pts[i].x, cur_right_pts[i].y), tmp_p);

            //             un_r_pts.push_back(cv::Point2f(tmp_p.x(), tmp_p.y()));
            //         }
            //     }


            // ids_right = ids;
            // reduceVector(cur_right_pts, status);
            // reduceVector(ids_right, status);
            // reduceVector(track_cnt_right, status);

            // assert(cur_right_pts.size() == un_l_pts.size()); 
            // vector<uchar> lr_fund_status(cur_right_pts.size(), 0); 

            // int cnt_right_inlier = 0; 
            // for(int i=0; i<lr_fund_status.size(); i++){
                
            //     // check epipilar distance 
            //     Vector3d p3d0(un_l_pts[i].x, un_l_pts[i].y, 1.);
            //     Vector3d p3d1(un_r_pts[i].x, un_r_pts[i].y, 1.);  
            //     const double epipolar_error =
            //     std::abs(p3d1.transpose() * Eeesntial_matrix_event * p3d0);
            //     if(epipolar_error < 0.005) // epipolar distance 
            //     {
            //         lr_fund_status[i] = 1; 
            //         ++cnt_right_inlier;
            //     }
            // }

            // reduceVector(cur_right_pts, lr_fund_status);
            // reduceVector(ids_right, lr_fund_status);

            // // fundamental matrix check // // 


            // 对所有成功匹配的右图像特征点，增加其跟踪计数
            for (auto &n : track_cnt_right)
                n++;

            // 为每个右图像特征点初始化跟踪计数
            // 注意：这里可能有逻辑问题，因为已经增加了现有点的计数，再为所有点添加计数=1可能不合理
            for (auto &p : cur_right_pts)
            {
                track_cnt_right.push_back(1);
            }

            // 计算右图像特征点的归一化坐标（去畸变）
            cur_un_right_pts = undistortedPts(cur_right_pts, stereo_m_camera[1]);
            
            // 计算右图像特征点的速度
            right_pts_velocity = ptsVelocity(ids_right, cur_un_right_pts, cur_un_right_pts_map, prev_un_right_pts_map);
        }
        
        // 更新前一帧右图像的归一化点映射
        prev_un_right_pts_map = cur_un_right_pts_map;
    }

    // 创建当前帧左图像特征点的ID映射表（ID到坐标的映射）
    curLeftPtsMap.clear();
    for(size_t i = 0; i < cur_pts.size(); i++)
        curLeftPtsMap[ids[i]] = cur_pts[i];

    // 创建当前帧右图像特征点的ID映射表
    curRightPtsMap.clear();
    for(size_t i = 0; i < cur_right_pts.size(); i++)
        curRightPtsMap[ids_right[i]] = cur_right_pts[i];

    // 更新前一帧的图像、特征点和时间戳，为下一次跟踪做准备
    prev_img_left = cur_img_left;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    prev_un_pts_map = cur_un_pts_map;
    prev_time = cur_time;
    prev_event_mat_left = event_mat_left;

    // 如果需要显示跟踪结果
    if(SHOW_TRACK){
        // 在事件图像上绘制立体跟踪结果
        stereo_event_drawTrack(event_mat_left, event_mat_right, ids, cur_pts, cur_right_pts, prevLeftPtsMap);
        
        // 在时间面上绘制前后帧跟踪结果
        event_drawTrack_two(cur_img_left, prev_img_left, ids, cur_pts, prev_pts, prevLeftPtsMap);
        
        // 在时间面上绘制左右图像匹配结果
        event_drawTrack_stereo(cur_img_left, cur_img_right, prev_img_left, ids, ids_right, cur_pts, cur_right_pts, curLeftPtsMap, curRightPtsMap);
    }

    // 更新前一帧左图像的特征点ID映射表
    prevLeftPtsMap.clear();
    for(size_t i = 0; i < cur_pts.size(); i++)
        prevLeftPtsMap[ids[i]] = cur_pts[i];

    return; // 函数结束
}

7.FeatureTracker::trackEvent函数（运动补偿）

与非运动补偿版的主要区别在于生成SAE的部分调用了的是运动补偿版本的SAE生成函数，这个函数位于event_detector.cc中，下面介绍此函数的运动补偿版本与非运动补偿版本

8.EventDetector::createSAE_left函数（运动补偿）

函数形式：void EventDetector::createSAE_left(double et, int ex, int ey, bool ep, const Motion_correction_value measurements)

需要首先明确的是SAE（Surface of Active Events，活动事件表面）存储了每个像素位置上最近的有效事件的时间戳。SAE分为两个通道，分别对应正极性和负极性事件。

两个比较重要的细节：

1.对于最新极性时间戳的更新：使用引用的方式：

// 获取该位置上同极性事件的最新时间戳引用
double & t_last = sae_latest_[pol](ex,ey);//save time// 同极性最新事件时间引用
// 获取该位置上反极性事件的最新时间戳引用
double & t_last_inv = sae_latest_[pol_inv](ex, ey);// 反极性最新事件时间引用

t_last 不是一个独立的变量，而是 sae_latest_pol 的引用，修改 t_last 时，实际上是在修改 sae_latest_[pol](ex,ey)，本质上来说，t_last 和 sae_latest_[pol](ex,ey) 是同一个变量的两个名字。

当执行 t_last = et; 时，实际上是在执行：sae_latest_pol = et;

这里使用引用大概是为了增加可读性，避免每次写sae_latest_pol 吧

2.对于sae的更新：

在下面的一段代码中实现了对sae的时间戳更新：

// 根据时间阈值和极性判断是否更新SAE
      if ((et > t_last + filter_threshold_) || (t_last_inv > t_last)) {
          // 如果当前事件时间比上次同极性事件时间晚过滤阈值
          // 或者上次反极性事件比上次同极性事件更晚
          // 则更新最新时间和SAE
          t_last = et;                  // 更新最新时间
          sae_[pol](ex, ey) = et;       // 更新SAE时间戳
      } else {
          // 否则只更新最新时间，不更新SAE
          // 这是为了过滤掉短时间内的重复事件
          t_last = et;
      }

主要影响TS的生成，因为只筛选了重要事件，记录其对应的时间戳，而不是所有事件的时间戳：sae_[pol](ex, ey) = et;// 更新SAE时间戳

后续的TS生成函数应该有对应转化，只使用 sae_[pol] 中的时间戳，在下一个函数中进行对应讲解。

void EventDetector::createSAE_left(double et, int ex, int ey, bool ep, const Motion_correction_value measurements){

      // 函数参数说明:
      // et: 事件的时间戳 (event time)
      // ex: 事件的x坐标 (event x-coordinate)
      // ey: 事件的y坐标 (event y-coordinate)
      // ep: 事件的极性 (event polarity) - true表示正极性，false表示负极性
      // measurements: 用于运动校正的测量值结构体

      // 保存事件的原始值作为常量，以便在校正后仍能访问原始值
      const double const_et = et;  // 原始事件时间戳
      const double const_ex = ex;  // 原始事件x坐标
      const double const_ey = ey;  // 原始事件y坐标

      // 从测量结构体中提取当前状态信息
      // 状态向量包含速度和时间信息
      Eigen::Vector4d State = measurements.second.first.first;  // 当前状态向量，包含速度和时间
      
      // 提取前一时刻的速度向量
      Eigen::Vector3f temp_v_pre = measurements.second.first.second;  // 前一时刻的速度
      
      // 定义并初始化当前速度向量
      Eigen::Vector3f temp_v;  // 当前速度
      temp_v[0] = State[0];  // x方向速度
      temp_v[1] = State[1];  // y方向速度
      temp_v[2] = State[2];  // z方向速度

      // 提取当前状态对应的时间戳
      double temp_time = State[3];  // 当前状态的时间戳

      // 从测量结构体中提取加速度和角速度信息
      Eigen::Vector3f accel_avg_ = measurements.second.second.second.first;   // 平均加速度
      Eigen::Vector3f omega_avg_ = measurements.second.second.second.second;  // 平均角速度(来自IMU)

      // 提取事件批次的时间范围
      double t_0 = measurements.second.second.first[0];// 当前事件批次的起始时间
      double t_1 = measurements.second.second.first[1];// 当前事件批次的结束时间
      const double dt = et - t_0; // 计算当前事件相对于批次起始时间的时间差 当前事件时间与批次起始时间的差值
      
      // 只有当加速度大于阈值时才进行运动校正
      // 这是为了避免在静止或匀速运动时不必要的计算
      if( sqrt(pow(accel_avg_[0], 2) + pow(accel_avg_[1], 2) + pow(accel_avg_[2], 2)) > a_motion_compensation_threshold)
      {        
        // 调用运动校正函数，获取校正后的坐标 核心todo motioncorrection
        Eigen::Vector2d correct_coordinate = motioncorrection(const_ex,const_ey,temp_v, // 原始坐标
                                                              temp_v_pre, // 当前和前一时刻的速度
                                                              accel_avg_, // 加速度
                                                              omega_avg_, // 角速度
                                                              dt); // 时间差
       
        ex=correct_coordinate[0];// 校正后的x坐标
        ey=correct_coordinate[1]; // 校正后的y坐标
      }     

      // 更新表面活动事件(SAE)
      // 确定事件极性对应的索引
      const int pol = ep ? 1 : 0;// 如果极性为正(ep=true)，则pol=1，否则pol=0
      const int pol_inv = (!ep) ? 1 : 0;// 相反极性的索引

      // 获取该位置上同极性事件的最新时间戳引用
      double & t_last = sae_latest_[pol](ex,ey);//save time// 同极性最新事件时间引用
      // 获取该位置上反极性事件的最新时间戳引用
      double & t_last_inv = sae_latest_[pol_inv](ex, ey);// 反极性最新事件时间引用

       // 根据时间阈值和极性判断是否更新SAE
      if ((et > t_last + filter_threshold_) || (t_last_inv > t_last)) {
          // 如果当前事件时间比上次同极性事件时间晚过滤阈值
          // 或者上次反极性事件比上次同极性事件更晚
          // 则更新最新时间和SAE
          t_last = et;                  // 更新最新时间
          sae_[pol](ex, ey) = et;       // 更新SAE时间戳
      } else {
          // 否则只更新最新时间，不更新SAE
          // 这是为了过滤掉短时间内的重复事件
          t_last = et;
      }

      // 在事件矩阵上标记事件位置和极性，用于可视化
      // 正极性事件显示为蓝色(255,0,0)，负极性事件显示为红色(0,0,255)
      cur_event_mat_left.at<cv::Vec3b>(cv::Point(ex, ey)) = (
      ep == true ? cv::Vec3b(255, 0, 0) : cv::Vec3b(0, 0, 255));
}

9.EventDetector::createSAE_left函数（非运动补偿）

只有运动补偿版本中的sae更新部分

10.EventDetector::motioncorrection函数

运动补偿核心数学原理：

应用罗德里格斯公式实现李群到李代数的指数映射计算旋转矩阵

omega_avg_对应论文中的:ω˜k - bg(tk) - ng(tk)，imu偏差在后端估计更新。

在stereo_event_tracker_node.cpp的handle_stereo_event函数中，角速度数据被获取：

// 获取IMU数据（角速度和线性加速度）
if (!imu_buf.empty()){
    omega_avg_[0] = imu_buf.front()->angular_velocity.x;
    omega_avg_[1] = imu_buf.front()->angular_velocity.y;
    omega_avg_[2] = imu_buf.front()->angular_velocity.z;
    accel_avg_[0] = imu_buf.front()->linear_acceleration.x;
    accel_avg_[1] = imu_buf.front()->linear_acceleration.y;
    accel_avg_[2] = imu_buf.front()->linear_acceleration.z - 9.805;  // 减去重力加速度
}

此时的omega_avg_是从IMU缓冲区获取的原始角速度数据，还没有校正偏差。

状态估计结果通过state_callback反馈到前端，前端使用这些信息构建运动补偿参数。

Eigen::Vector2d EventDetector::motioncorrection(const double ex,                    // 原始事件x坐标
                                                const double ey,                    // 原始事件y坐标
                                                const Eigen::Vector3f tmp_v,        // 当前时刻的速度向量
                                                const Eigen::Vector3f tmp_v_pre,    // 前一时刻的速度向量
                                                const Eigen::Vector3f accel_avg_,   // 平均加速度向量
                                                const Eigen::Vector3f omega_avg_,   // 平均角速度向量
                                                const double dt)                    // 时间增量

{
  Eigen::Vector2d correct_coordinate;    // 创建存储校正后坐标的向量
  const int kBorder = 6;                 // 定义传感器边界宽度常量

  if(ex > kBorder && ex <= (kSensorWidth_ - kBorder) &&
      ey > kBorder && ey <= (kSensorHeight_ - kBorder) ){// 只处理非边界区域的点 边界点容易引起插值问题，所以跳过它们

      Eigen::Vector3f rotation_vector = omega_avg_ * dt;  // 计算旋转向量 = 角速度 * 时间
      Eigen::Matrix3f rot_skew_mat = vectorToSkewMat(rotation_vector);  // 转换为反对称矩阵
      Eigen::Matrix3f rotation_matrix_ = rot_skew_mat.exp();  // 计算矩阵指数得到旋转矩阵

      
      // 计算旋转在相机坐标系中的表示
      Eigen::Matrix3f rot_K = intrinsics_matrix * rotation_matrix_.transpose() * intrinsics_matrix.inverse();

      // 计算平均位移向量
      Eigen::Vector3f trans_K = 0.5 * dt * (tmp_v + tmp_v_pre);

      // 调整变换向量（注意负号）
      trans_K = -rot_K * (intrinsics_matrix.inverse() * trans_K);     

      // 构建事件原始坐标的齐次坐标
      Eigen::Vector3f eventVec;
      eventVec[0] = ex;
      eventVec[1] = ey;
      eventVec[2] = 1;

      // 应用旋转和平移变换，将事件坐标映射到参考时刻   将事件坐标变换到t_0时刻
      eventVec = rot_K * eventVec+trans_K;  // event warp to t_0

      // 转换回齐次坐标  归一化齐次坐标，确保最后一个分量为1
      ConvertToHomogeneous(&eventVec);//homogeneous

      // 取整数坐标
      int x_coordinate = std::floor(eventVec[0]);
      int y_coordinate = std::floor(eventVec[1]);

      // 检查变换后的坐标是否仍在传感器范围内
      if(x_coordinate>0 && x_coordinate< kSensorWidth_ -1&& y_coordinate >0 && y_coordinate <kSensorHeight_-1)//avoid out of range
      {
        // 如果在有效范围内，使用校正后的坐标
        correct_coordinate[0]=x_coordinate;
        correct_coordinate[1]=y_coordinate;
      }    
      else{
        // 如果超出范围，回退到原始坐标
        correct_coordinate[0] = ex;
        correct_coordinate[1] = ey;
      }
  
  }
  else{
    // 对于边界点，保持原始坐标不变
    correct_coordinate[0] = ex;
    correct_coordinate[1] = ey;
  }

  return correct_coordinate; // 返回校正后的坐标
}

11.FeatureTracker::Image_setMask函数

这个函数通过以下步骤实现特征点的均匀分布：

  1. 初始化一个掩码图像，标记所有可能放置特征点的区域

  2. 按跟踪次数对特征点排序，优先保留稳定的特征点

  3. 遍历排序后的特征点，在掩码允许的区域保留特征点

  4. 每保留一个特征点，就在其周围画一个黑色圆形，防止其他特征点太靠近

  5. 最终得到一组均匀分布且优先保留长时间跟踪特征点的集合

这种策略确保了特征点在图像中分布均匀，提高了视觉SLAM系统的稳定性和准确性。类似于ORB中的四叉树特征点均匀化

这里我觉得应该搞清楚的几个点是掩码区域后续的应用和特征点跟踪计数是如何与坐标以及ID对应的：

首先是掩码对特征检测的影响，在trackImage函数中，掩码被传递给特征检测函数：

// 使用Shi-Tomasi角点检测器在掩码允许的区域检测新特征点
cv::goodFeaturesToTrack(cur_img_left, n_pts, MAX_CNT_IMG - cur_pts.size(), 0.01, MIN_DIST_IMG, mask_image);

goodFeaturesToTrack函数只会在掩码值为非零（白色区域）的位置检测特征点，完全忽略掩码值为零（黑色区域）的位置。

然后是第二个问题，vector track_cnt;//total track num 这是track_cnt的定义，这是一个int类型的容器，那么怎么知道是哪个特征点检测的次数呢？

代码中，特征点信息通过多个平行数组（parallel arrays）来存储：

vector<cv::Point2f> cur_pts;    // 当前帧特征点坐标
vector<int> ids;                // 特征点的唯一ID
vector<int> track_cnt;          // 特征点的跟踪计数

这三个向量保持索引对应，这意味着：

cur_pts[i] - 第i个特征点的坐标
ids[i] - 第i个特征点的ID
track_cnt[i] - 第i个特征点的跟踪计数

系统通过严格维护这三个向量的同步来确保对应关系：同步添加：新特征点被添加时，三个向量同时更新


cur_pts.push_back(p);       // 添加坐标
ids.push_back(n_id++);      // 添加ID
track_cnt.push_back(1);     // 添加跟踪计数

同步删除：特征点被移除时，三个向量同时更新


reduceVector(prev_pts, status);  // 根据status移除特征点
reduceVector(cur_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);

同步遍历：处理特征点时，通过索引保持一致


for (unsigned int i = 0; i < cur_pts.size(); i++){
    cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(cur_pts[i], ids[i])));
}

reduceVector 函数是维护这种对应关系的关键：同步过滤多个向量：当光流跟踪失败时，需要同时从多个向量（特征点坐标、ID、跟踪计数等）中移除对应元素

// 根据状态向量移除容器中的元素
template <typename T>
void reduceVector(vector<T> &v, vector<uchar> status)
{
    int j = 0;
    for (size_t i = 0; i < v.size(); i++)
        if (status[i])
            v[j++] = v[i];
    v.resize(j);
}

这个函数确保了：

在实际代码中，这个函数通常会被实现为模板函数，以便能够处理不同类型的向量：

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

void FeatureTracker::Image_setMask()
{
    // 如果使用鱼眼相机，则使用预定义的鱼眼掩码；否则创建一个全255（全白）的掩码 CV_8UC1表示8位无符号单通道图像
    if(FISHEYE)
        mask_image = fisheye_mask.clone();
    else
        mask_image = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));

    // 创建一个容器，存储每个特征点的跟踪次数、坐标和ID
    vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;

    // 将当前所有特征点的信息添加到容器中
    for (unsigned int i = 0; i < cur_pts.size(); i++){
        cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(cur_pts[i], ids[i])));
    }
    
    // 按跟踪次数从高到低排序特征点
    /*
        使用lambda表达式作为比较函数，对cnt_pts_id容器中的元素进行排序
        比较函数return a.first > b.first确保按照特征点的跟踪次数（第一个元素）从大到小排序
        这样做的目的是优先保留那些已经被成功跟踪多次的特征点，因为它们可能更稳定
    */
    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;
            });

    cur_pts.clear();  // 清空当前的特征点容器，准备重新填充
    ids.clear();
    track_cnt.clear();

    // 遍历排序后的特征点，根据掩码决定是否保留
    for (auto &it : cnt_pts_id)
    {
        // 检查该点在掩码中是否为255（可用区域）
        if (mask_image.at<uchar>(it.second.first) == 255)
        {
            // 如果可用，则保留该特征点
            cur_pts.push_back(it.second.first);
            ids.push_back(it.second.second);
            track_cnt.push_back(it.first);

            // 在该特征点周围画一个半径为MIN_DIST_IMG的黑色圆形（值为0）
            cv::circle(mask_image, it.second.first, MIN_DIST_IMG, 0, -1);
        }
    }
}

12.FeatureTracker::Event_setMask函数

和image的基本相同，不多加赘述了

唯一的区别在于出事话的有效区域值为0，这与Image的mask相反（255）,检测事件角点的时候利用了这一设置，不要混淆了。

13.EventDetector::SAEtoTimeSurface_left函数

有一个比较有意思的点值得注意：

与ESVO2不同的一点是这里在生成TS的时候可以选择是否使用极性信息，所生成的TS中：

在考虑极性的Time Surface中，没有事件的区域：most_recent_stamp_at_coordXY为0，因此指数衰减值expVal也为0

值域映射：当expVal = 0时，映射后的值为255.0 * (0 + 1.0) / 2.0 = 127.5，四舍五入后在8位图像中显示为灰色(128)

好处：边缘特征会更突出（相比于不考虑极性的只有黑白两色的TS来说），但这种好处的量化效益还需要进一步验证（待做）。

cv::Mat EventDetector::SAEtoTimeSurface_left( const double external_sync_time){

    // create exponential-decayed Time Surface map
    const double decay_sec = decay_ms_ / 1000.0;//ms to s
    cv::Mat time_surface_map;
    time_surface_map=cv::Mat::zeros(sensor_size_, CV_64F); //Time Surface矩阵首先被初始化为全零矩阵

    for (int y=0;y<sensor_size_.height;++y){
        for(int x=0;x<sensor_size_.width;++x){

              double most_recent_stamp_at_coordXY= (sae_[1](x,y)>sae_[0](x,y)) ? sae_[1](x,y) : sae_[0](x,y);

              if(most_recent_stamp_at_coordXY > 0){
                    const double dt = (external_sync_time-most_recent_stamp_at_coordXY);
                    double expVal =std::exp(-dt / decay_sec);

                // 极性处理：如果不忽略极性(ignore_polarity_为false)
                if (!ignore_polarity_) {
                    // 确定当前位置的极性
                    double polarity = (sae_[1](x,y) > sae_[0](x,y)) ? 1.0 : -1.0;
                    
                    // 将极性值与指数衰减值相乘
                    // 这样正极性事件产生正值，负极性事件产生负值
                    expVal *= polarity;
                }
                
                // 将计算好的值存储到时间表面图中
                // 注意OpenCV的矩阵访问顺序是(y,x)而非(x,y)
                time_surface_map.at<double>(y, x) = expVal;
            }
        }
    }

    // 将时间表面值映射到适合图像显示的范围[0,255]
    if (!ignore_polarity_) {
        // 如果考虑极性：将[-1,1]范围映射到[0,255]
        // (expVal + 1.0) / 2.0 将[-1,1]映射到[0,1]
        // 然后乘以255将[0,1]映射到[0,255]
        time_surface_map = 255.0 * (time_surface_map + 1.0) / 2.0;
    } else {
        // 如果忽略极性：将[0,1]范围映射到[0,255]
        // 直接乘以255即可
        time_surface_map = 255.0 * time_surface_map;
    }
    time_surface_map.convertTo(time_surface_map, CV_8U);

    // 可选的中值滤波处理
    // 如果设置了中值滤波核大小(大于0)，对图像应用中值滤波
    // 中值滤波可以有效去除椒盐噪声，保留边缘
    if (median_blur_kernel_size_ > 0) {
        // 计算实际的核大小：2*size+1确保核大小为奇数(OpenCV要求)
        // 例如，如果median_blur_kernel_size_=1，则核大小为3x3
        cv::medianBlur(time_surface_map, time_surface_map, 2 * median_blur_kernel_size_ + 1);
    }

  return time_surface_map;
}

14.Event_FeaturesToTrack函数

需要重点关注后续的isCorner函数，其他逻辑是比较直观的。

void Event_FeaturesToTrack( const dvs_msgs::EventArray &last_event,  
                            vector<cv::Point2f> &n_pts, 
                            const vector<cv::Point2f> &_cur_pts, 
                            const int maxCorners, 
                            const int _MIN_DIST, 
                            const cv::Mat event_mask, 
                            const cv::Mat time_surface_map)
{
    // 函数功能：在事件数据中检测角点特征用于跟踪
    // 参数说明：
    // last_event：     输入的事件数组，包含多个事件点
    // n_pts：          输出参数，存储检测到的角点特征位置
    // _cur_pts：       当前已有的特征点，用于避免重复检测
    // maxCorners：     最大允许检测的角点数量
    // _MIN_DIST：      角点之间的最小距离，确保特征分布均匀
    // event_mask：     掩码矩阵，标记哪些区域不应检测特征
    // time_surface_map：时间表面图，用于特征质量判断

    int ncorners = 0; // 初始化角点计数器为0，用于统计已检测到的角点数量
    n_pts.clear();// 清空输出角点容器，准备存储新检测的角点
    cv:: Mat event_mask_cur=event_mask.clone(); // 复制输入掩码 值为255.0的位置不会被检测

    if(maxCorners>0){//when need to detect
    for (const dvs_msgs::Event& e : last_event.events) { // 遍历输入事件数组中的每个事件

        if (ncorners>=maxCorners) {// 如果已检测角点数达到或超过要求的最大数量，则退出循环
                break;
            } 
            else{
                // 检查当前事件位置在掩码中的值是否不等于255.0
                // 如果不等于255.0，说明该位置允许检测角点
                if(event_mask_cur.at<double>(e.y,e.x)!=255.0 ){
                    // 检查该位置在时间表面图中的值是否不等于阈值TS_LK_THRESHOLD
                    // 这是对时间表面值的筛选，确保只在有效的事件位置检测角点
                     if(time_surface_map.at<uchar>(e.y,e.x) !=TS_LK_THRESHOLD)
                     { 
                        if (detector.isCorner(e.ts.toSec(), e.x, e.y, e.polarity)) //调用角点检测器判断当前事件是否为角点
                        {
                            // 传入事件的时间戳、x坐标、y坐标和极性  
                            n_pts.push_back(cv::Point2f((float)e.x, (float)e.y)); // 如果是角点，将其坐标添加到输出角点容器中
                            ncorners++;//角点计数器增加1
                            cv::circle(event_mask_cur, cv::Point(e.x,e.y), _MIN_DIST, 255.0, -1);//在角点位置周围画一个实心圆，半径为_MIN_DIST，值为255.0
                        }
                     }                
                }
            }

        }            
    }              
}

15.EventDetector::isCorner函数

ESVIO前端的核心算法，用于检测特征点，这里作者使用了下面这篇文章中提出的Arc事件特征：

@ARTICLE{8392795,
  author={Alzugaray, Ignacio and Chli, Margarita},
  journal={IEEE Robotics and Automation Letters}, 
  title={Asynchronous Corner Detection and Tracking for Event Cameras in Real Time}, 
  year={2018},
  volume={3},
  number={4},
  pages={3177-3184},
  keywords={Cameras;Tracking;Corner detection;Real-time systems;Feature extraction;Robot vision systems;Simultaneous localization and mapping;Visual tracking;computer vision for other robotic applications;SLAM},
  doi={10.1109/LRA.2018.2849882}}

该特征点基于Fast角点改进而来，使用SAE平面，以时间戳大小代替像素点强度来进行角点检测，前序文章是Scaramuzza组提出的eFast角点。核心思想是认为当某一个像素周围两个内外圆形区域上的时间戳圆弧满足特定长度要求时将其判定为Arc特征，从论文中的图中能比较直观的体会到。

Arc角点弥补了eFast角点只能检测出圆弧角度小于180°的缺陷（这种设计的初衷是避免将直线上的事件判定为特征），通过对大小圆弧进行双边判别确保无论物体朝什么方向运动都能够较为准确地实现特征检测。

bool EventDetector::isCorner(double et, int ex, int ey, bool ep) {

    const int pol = ep ? 1 : 0;
    const int pol_inv = (!ep) ? 1 : 0;
    double & t_last = sae_latest_[pol](ex,ey);
    double & t_last_inv = sae_latest_[pol_inv](ex, ey);

    if ((et > t_last + filter_threshold_) || (t_last_inv > t_last) ) {
      return false;
    }

    // 边界检查：如果事件太靠近图像边界，返回false
    // 因为角点检测需要检查周围像素，靠近边界的事件无法完成完整检查
    const int kBorderLimit = MIN_DIST + 1;
    if (ex < kBorderLimit || ex >= (kSensorWidth_ - kBorderLimit) ||
        ey < kBorderLimit || ey >= (kSensorHeight_ - kBorderLimit)) {
      return false;
    }

    // 定义角点检测的常量和阈值：
    // - 小圆和大圆的大小（采样点数量）
    // - 小圆和大圆上最新段的最小和最大长度阈值
    const int kSmallCircleSize = 16;
    const int kLargeCircleSize = 20;
    const int kSmallMinThresh = 4;
    const int kSmallMaxThresh = 6;
    const int kLargeMinThresh = 5;
    const int kLargeMaxThresh = 8;


    bool is_arc_valid = false; // 标记是否找到有效的角点特征（弧段

    // 初始化"最新段"的最小时间戳，从小圆第一个采样点开始
    double segment_new_min_t = sae_[pol](ex+kSmallCircle_[0][0], ey+kSmallCircle_[0][1]);

    // Left and Right are equivalent to CW and CCW as in the paper
    // 定义左右弧段的索引（顺时针和逆时针方向）
    int arc_right_idx = 0;
    int arc_left_idx;

    // 在小圆上找到时间戳最新（最大）的点，并记录其索引
    for (int i=1; i<kSmallCircleSize; i++) {
      const double t =sae_[pol](ex+kSmallCircle_[i][0], ey+kSmallCircle_[i][1]);
      if (t > segment_new_min_t) {
        segment_new_min_t = t;
        arc_right_idx = i; 
      }
    }

    // 将左右弧段索引移动到最新点的两侧（使用取模运算处理环形结构）
    arc_left_idx = (arc_right_idx-1+kSmallCircleSize)%kSmallCircleSize;
    arc_right_idx= (arc_right_idx+1)%kSmallCircleSize;

    // 获取左右弧段起始点的时间戳值
    double arc_left_value = sae_[pol](ex+kSmallCircle_[arc_left_idx][0], ey+kSmallCircle_[arc_left_idx][1]);
    double arc_right_value = sae_[pol](ex+kSmallCircle_[arc_right_idx][0], ey+kSmallCircle_[arc_right_idx][1]);

    // 初始化左右弧段的最小时间戳（用于后续更新）
    double arc_left_min_t = arc_left_value;
    double arc_right_min_t = arc_right_value;

    // Expand
    // Initial expand does not require checking
    int iteration = 1; // The arc already contain the maximum
    for (; iteration<kSmallMinThresh; iteration++) { // 第一阶段扩展：必须扩展到最小阈值kSmallMinThresh
      // Decide the most promising arc
      if (arc_right_value > arc_left_value) { // 如果右弧段的时间戳更新（更大），则扩展右弧段
        if (arc_right_min_t < segment_new_min_t) { 
            segment_new_min_t = arc_right_min_t; // 如果右弧段的最小值小于当前"最新段"的最小值，更新"最新段"的最小值
        }
        // Expand arc
        arc_right_idx= (arc_right_idx+1)%kSmallCircleSize;// 向右扩展弧段，并获取新点的时间戳
        arc_right_value = sae_[pol](ex+kSmallCircle_[arc_right_idx][0], ey+kSmallCircle_[arc_right_idx][1]);

        if (arc_right_value < arc_right_min_t) { // 更新右弧段的最小时间戳
          arc_right_min_t = arc_right_value;
        }
      } else { // 如果左弧段的时间戳更新（更大），则扩展左弧段
        // Include arc in new segment
        if (arc_left_min_t < segment_new_min_t) {
          segment_new_min_t = arc_left_min_t;
        }

        // Expand arc
        arc_left_idx= (arc_left_idx-1+kSmallCircleSize)%kSmallCircleSize;
        arc_left_value = sae_[pol](ex+kSmallCircle_[arc_left_idx][0], ey+kSmallCircle_[arc_left_idx][1]);

        if (arc_left_value < arc_left_min_t) { // Update minimum of the arc
          arc_left_min_t = arc_left_value;
        }
      }
    }
    int newest_segment_size = kSmallMinThresh;// 记录"最新段"的当前大小，初始为最小阈值

    // 第二阶段扩展：继续扩展直到完成整个圆
    for (; iteration<kSmallCircleSize; iteration++) {

      // 如果右弧段的时间戳更新，则扩展右弧段
      if (arc_right_value > arc_left_value) 
      {  
        if ((arc_right_value >=  segment_new_min_t)) 
        { 
          // 如果右弧段的值大于等于"最新段"的最小值，将其包含在"最新段"中
          newest_segment_size = iteration+1; // 更新"最新段"的大小
          if (arc_right_min_t < segment_new_min_t) {
            segment_new_min_t = arc_right_min_t;
          }
        }

        // 向右扩展弧段，并获取新点的时间戳
        arc_right_idx= (arc_right_idx+1)%kSmallCircleSize;
        arc_right_value = sae_[pol](ex+kSmallCircle_[arc_right_idx][0], ey+kSmallCircle_[arc_right_idx][1]);

        if (arc_right_value < arc_right_min_t) // 更新右弧段的最小时间戳
        { // Update minimum of the arc
          arc_right_min_t = arc_right_value;
        }
      } 
      else 
      { 
        // 如果左弧段的值大于等于"最新段"的最小值，将其包含在"最新段"中
        if ((arc_left_value >=  segment_new_min_t)) 
        {
          newest_segment_size = iteration+1;
          if (arc_left_min_t < segment_new_min_t) {
            segment_new_min_t = arc_left_min_t;
          }
        }

        // Expand arc
        arc_left_idx= (arc_left_idx-1+kSmallCircleSize)%kSmallCircleSize;
        arc_left_value = sae_[pol](ex+kSmallCircle_[arc_left_idx][0], ey+kSmallCircle_[arc_left_idx][1]);
        if (arc_left_value < arc_left_min_t) { // Update minimum of the arc
          arc_left_min_t = arc_left_value;
        }
      }
    }

    // 检查小圆上的"最新段"大小是否满足角点条件：
    // 1. "最新段"很小（<=kSmallMaxThresh）
    // 2. 或者"最新段"非常大（但不是整个圆）
    // 如果满足任一条件，将is_arc_valid标记为true

    if (// Corners with newest segment of a minority of elements in the circle
        // These corners are equivalent to those in Mueggler et al. BMVC17
            (newest_segment_size <= kSmallMaxThresh) ||
        // Corners with newest segment of a majority of elements in the circle
        // This can be commented out to decrease noise at expenses of less repeatibility. If you do, DO NOT forget to comment the equilvent line in the large circle
        ((newest_segment_size >= (kSmallCircleSize - kSmallMaxThresh)) && (newest_segment_size <= (kSmallCircleSize - kSmallMinThresh)))) 
    {
      is_arc_valid = true;
    }

    /// 如果小圆检测通过，重置is_arc_valid并进行大圆检测
    if (is_arc_valid) {
    is_arc_valid = false;

      segment_new_min_t = sae_[pol](ex+kLargeCircle_[0][0], ey+kLargeCircle_[0][1]);
      arc_right_idx = 0;

      // Initialize in the newest element
      for (int i=1; i<kLargeCircleSize; i++) {
        const double t =sae_[pol](ex+kLargeCircle_[i][0], ey+kLargeCircle_[i][1]);
        if (t > segment_new_min_t) {
          segment_new_min_t = t;
          arc_right_idx = i; // % End up in the maximum value
        }
      }
      // Shift to the sides of the newest elements;
      arc_left_idx = (arc_right_idx-1+kLargeCircleSize)%kLargeCircleSize;
      arc_right_idx= (arc_right_idx+1)%kLargeCircleSize;
      arc_left_value = sae_[pol](ex+kLargeCircle_[arc_left_idx][0],
                                 ey+kLargeCircle_[arc_left_idx][1]);
      arc_right_value = sae_[pol](ex+kLargeCircle_[arc_right_idx][0],
                                  ey+kLargeCircle_[arc_right_idx][1]);
      arc_left_min_t = arc_left_value;
      arc_right_min_t = arc_right_value;

      // Expand
      // Initial expand does not require checking
      iteration = 1;
      for (; iteration<kLargeMinThresh; iteration++) {
        // Decide the most promising arc
        if (arc_right_value > arc_left_value) { // Right arc
          if (arc_right_min_t < segment_new_min_t) {
              segment_new_min_t = arc_right_min_t;
          }
          // Expand arc
          arc_right_idx= (arc_right_idx+1)%kLargeCircleSize;
          arc_right_value = sae_[pol](ex+kLargeCircle_[arc_right_idx][0],
                                      ey+kLargeCircle_[arc_right_idx][1]);
          if (arc_right_value < arc_right_min_t) { // Update minimum of the arc
            arc_right_min_t = arc_right_value;
          }
        } else { // Left arc
          // Include arc in new segment
          if (arc_left_min_t < segment_new_min_t) {
            segment_new_min_t = arc_left_min_t;
          }

          // Expand arc
          arc_left_idx= (arc_left_idx-1+kLargeCircleSize)%kLargeCircleSize;
          arc_left_value = sae_[pol](ex+kLargeCircle_[arc_left_idx][0],
                                     ey+kLargeCircle_[arc_left_idx][1]);
          if (arc_left_value < arc_left_min_t) { // Update minimum of the arc
            arc_left_min_t = arc_left_value;
          }
        }
      }
      newest_segment_size = kLargeMinThresh;

      // Further expand until completion of the circle
      for (; iteration<kLargeCircleSize; iteration++) {
        // Decide the most promising arc
        if (arc_right_value > arc_left_value) { // Right arc
          // Include arc in new segment
          if ((arc_right_value >=  segment_new_min_t)) {
            newest_segment_size = iteration+1;
            if (arc_right_min_t < segment_new_min_t) {
              segment_new_min_t = arc_right_min_t;
            }
          }

          // Expand arc
          arc_right_idx= (arc_right_idx+1)%kLargeCircleSize;
          arc_right_value = sae_[pol](ex+kLargeCircle_[arc_right_idx][0],
                                      ey+kLargeCircle_[arc_right_idx][1]);
          if (arc_right_value < arc_right_min_t) { // Update minimum of the arc
            arc_right_min_t = arc_right_value;
          }
        } else { // Left arc
          // Include arc in new segment
          if ((arc_left_value >=  segment_new_min_t)) {
            newest_segment_size = iteration+1;
            if (arc_left_min_t < segment_new_min_t) {
              segment_new_min_t = arc_left_min_t;
            }
          }

          // Expand arc
          arc_left_idx= (arc_left_idx-1+kLargeCircleSize)%kLargeCircleSize;
          arc_left_value = sae_[pol](ex+kLargeCircle_[arc_left_idx][0],
                                    ey+kLargeCircle_[arc_left_idx][1]);
          if (arc_left_value < arc_left_min_t) { // Update minimum of the arc
            arc_left_min_t = arc_left_value;
          }
        }
      }

      // 检查大圆上的"最新段"大小是否满足角点条件：
      // 1. "最新段"很小（<=kLargeMaxThresh）
      // 2. 或者"最新段"非常大（但不是整个圆）
      // 如果满足任一条件，将is_arc_valid标记为true
      
      if (// Corners with newest segment of a minority of elements in the circle
          // These corners are equivalent to those in Mueggler et al. BMVC17
              (newest_segment_size <= kLargeMaxThresh) ||
          // Corners with newest segment of a majority of elements in the circle
          // This can be commented out to decrease noise at expenses of less repeatibility. If you do, DO NOT forget to comment the equilvent line in the small circle
          (newest_segment_size >= (kLargeCircleSize - kLargeMaxThresh) && (newest_segment_size <= (kLargeCircleSize - kLargeMinThresh))) ) {
        is_arc_valid = true;
      }
    }

    return is_arc_valid;
}

16.两个EventDetector::init函数

调用的是哪个版本取决于Do_motion_correction参数：

当Do_motion_correction == 0时，调用基本版本init(col, row)

当Do_motion_correction != 0时，调用扩展版本init(col, row, fx, fy, cx, cy)

//只初始化传感器尺寸和基本参数,不设置相机内参矩阵
void EventDetector::init(int col, int row){ 

    kSensorWidth_=col;
    kSensorHeight_=row;
    sensor_size_ = cv::Size(kSensorWidth_, kSensorHeight_);
    sae_[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_left[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_left[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_right[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_right[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_left[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_left[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_right[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_right[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);

    decay_ms_=para_decay_ms;
    ignore_polarity_= (bool) para_ignore_polarity;
    median_blur_kernel_size_=para_median_blur_kernel_size;
    filter_threshold_=para_feature_filter_threshold;

}

//初始化传感器尺寸和基本参数,额外设置相机内参矩阵 需要相机内参进行运动校正，所以使用带内参版本的init
void EventDetector::init(int col, int row, const double fx, const double fy, const double cx, const double cy){

    kSensorWidth_=col;
    kSensorHeight_=row;
    sensor_size_ = cv::Size(kSensorWidth_, kSensorHeight_);
    sae_[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_left[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_left[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_right[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_right[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_left[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_left[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_right[0] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);
    sae_latest_right[1] = Eigen::MatrixXd::Zero(kSensorWidth_, kSensorHeight_);

    decay_ms_=para_decay_ms;
    ignore_polarity_= (bool) para_ignore_polarity;
    median_blur_kernel_size_=para_median_blur_kernel_size;
    filter_threshold_=para_feature_filter_threshold;
    
    intrinsics_matrix << fx, 0.,         cx,
                         0.,         fy, cy,
                         0.,         0.,          1.;

    cout << intrinsics_matrix << endl;
}

17.无参和有参构造函数

在feature_tracker.cpp中，检测器是这样声明和创建的：

esvio::EventDetector detector = esvio::EventDetector();

代码中没有用到有参构造函数,使用的是无参构造函数，只初始化了常量成员变量。然后在stereo_event_tracker_node.cpp中，根据Do_motion_correction参数调用不同版本的init函数完成完整初始化：

if(FLAG_DETECTOR_NOSTART){
    FLAG_DETECTOR_NOSTART = false;
    if(Do_motion_correction == 0)
        detector.init(COL_event, ROW_event);
    else
        detector.init(COL_event, ROW_event, fx, fy, cx, cy);
}

18.FeatureTracker::rejectWithF_event函数

这里想简单说一下基于RANSAC的外点剔除，作者这里调用了OpenCV的findFundamentalMat函数计算基础矩阵并判定内外点：

cv::findFundamentalMat(un_prev_pts, un_cur_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);

输入是前后两帧的特征点，输出则是status。值得学习的是归一化的操作。

void FeatureTracker::rejectWithF_event()// left cur, left pre
{
    if (cur_pts.size() >= 8)// 8点法RANSAC
    {
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;
        vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_prev_pts(prev_pts.size());
        for (unsigned int i = 0; i < prev_pts.size(); i++)
        {
            Eigen::Vector3d tmp_p;
            // 处理前一帧的点并归一化
            /*
            去畸变处理：liftProjective函数将像素坐标投影到归一化相机平面，去除相机镜头畸变影响。
            归一化坐标转换：将归一化平面坐标转换回像素坐标，但使用统一的焦距FOCAL_LENGTH而非实际相机参数,s
            这使得不同相机或不同分辨率的图像可以在相同的尺度下比较。
            中心偏移：加上COL_event / 2.0和ROW_event / 2.0将坐标原点移到图像中心。
            */
            stereo_m_camera[0]->liftProjective(Eigen::Vector2d(prev_pts[i].x, prev_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL_event / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW_event / 2.0;
            un_prev_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());

            // 处理当前帧的点并归一化
            stereo_m_camera[0]->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_event / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW_event / 2.0;
            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
        }

        vector<uchar> status;
        // 使用OpenCV的findFundamentalMat函数计算基础矩阵：
        cv::findFundamentalMat(un_prev_pts, un_cur_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = prev_pts.size();//prev points size
        // 根据RANSAC的结果，使用reduceVector函数移除所有被标记为外点(status=0)的特征点
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(cur_un_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
        // ROS_INFO("FM ransac for the left point: %d -> %lu: %f%", size_a, cur_pts.size(), 100.0*cur_pts.size()/size_a);

    }
}

功能包2：esvio_estimator(位姿估计)

包含esio_estimator_node.cpp、stereo_estimator_node.cpp、estimator.cpp、feature_manager.cpp、parameters.cpp五个核心文件。

包含两个节点：stereo_esvio_estimator 和 stereo_esio_estimator节点，这两个节点分别对应ESVIO和ESIO两种工作模式。

首先分析ESVIO工作模式，这是双目+event+IMU进行位姿估计的核心模块。

1.stereo_estimator_node的main函数