diff --git a/_posts/2025-01-17-ESVO2.md b/_posts/2025-01-17-ESVO2.md index cfc060b..0c727c2 100644 --- a/_posts/2025-01-17-ESVO2.md +++ b/_posts/2025-01-17-ESVO2.md @@ -146,7 +146,197 @@ type: Brief introduction **12.检查文件是否存在函数ImageRepresentation::fileExists** -### ✨核心功能——生成时间表面TimeSurface以及自适应累积图AA map +### ✨1. 核心功能——生成时间表面TimeSurface以及自适应累积图AA map + +头文件ImageeRepresentation.h: + +``` +#ifndef image_representation_H_ +#define image_representation_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace image_representation +{ + // 定义全局事件队列,使用map存储,key为时间戳,value为事件数据 + using EventQueue = std::deque; + + // 定义时间比较结构体,用于map排序 + struct ROSTimeCmp + { + bool operator()(const ros::Time &a, const ros::Time &b) const + { + return a.toNSec() < b.toNSec(); // 按纳秒级时间戳比较 + } + }; + + // 定义全局事件队列,使用map存储,key为时间戳,value为事件数据 + using GlobalEventQueue = std::map; + + // 在事件队列中找到第一个时间戳不小于给定时间t的事件位置 + inline static EventQueue::iterator EventBuffer_lower_bound( + EventQueue &eb, ros::Time &t) + { + return std::lower_bound(eb.begin(), eb.end(), t, + [](const dvs_msgs::Event &e, const ros::Time &t) + { return e.ts.toSec() < t.toSec(); }); + } + + // 在事件队列中找到第一个时间戳大于给定时间t的事件位置 + inline static EventQueue::iterator EventBuffer_upper_bound( + EventQueue &eb, ros::Time &t) + { + return std::upper_bound(eb.begin(), eb.end(), t, + [](const ros::Time &t, const dvs_msgs::Event &e) + { return t.toSec() < e.ts.toSec(); }); + } + + // 在事件向量中找到第一个时间戳不小于给定时间t的事件位置 + inline static std::vector::iterator EventVector_lower_bound( + std::vector &ev, double &t) + { + return std::lower_bound(ev.begin(), ev.end(), t, + [](const dvs_msgs::Event &e, const double &t) + { return e.ts.toSec() < t; }); + } + + class ImageRepresentation + { + public: + ImageRepresentation(ros::NodeHandle &nh, ros::NodeHandle nh_private); + virtual ~ImageRepresentation(); + + // 时间比较静态方法 + static bool compare_time(const dvs_msgs::Event &e, const double reference_time) + { + return reference_time < e.ts.toSec(); + } + + private: + ros::NodeHandle nh_; + // core 初始化图像表示系统,设置图像尺寸和相关数据结构 + void init(int width, int height); + + // 在指定时间点创建事件图像表示 Support: TS, AA, negative_TS, negative_TS_dx, negative_TS_dy + void createImageRepresentationAtTime(const ros::Time &external_sync_time); + + // 图像生成的主循环函数,持续处理事件并生成不同类型的图像表示 + void GenerationLoop(); + + // callbacks 事件回调函数,处理接收到的事件数组 + void eventsCallback(const dvs_msgs::EventArray::ConstPtr &msg); + + // utils 清空事件队列,释放内存 + void clearEventQueue(); + // 加载相机标定信息 is_left: 是否为左相机 返回: 加载是否成功 + bool loadCalibInfo(const std::string &cameraSystemDir, bool &is_left); + // 清理指定距离内的事件 + void clearEvents(int distance, std::vector::iterator ptr_e); + + // AA(Adaptive Accumulation)处理线程 + void AA_thread(std::vector::iterator &ptr_e, int distance, double external_t); + // Sobel边缘检测处理函数 + void sobel(double external_t); + // 检查文件是否存在 + bool fileExists(const std::string &filename); + // tests + + // calibration parameters + cv::Mat camera_matrix_, dist_coeffs_; + cv::Mat rectification_matrix_, projection_matrix_; + std::string distortion_model_; // 畸变模型类型 + cv::Mat undistort_map1_, undistort_map2_; + Eigen::Matrix2Xd precomputed_rectified_points_; // 预计算的校正点 + + // sub & pub + ros::Subscriber event_sub_; + ros::Subscriber camera_info_sub_; + + image_transport::Publisher dx_image_pub_, dy_image_pub_; // 梯度图像发布器 + image_transport::Publisher image_representation_pub_TS_; + image_transport::Publisher image_representation_pub_negative_TS_; // 负时间表面发布器 + image_transport::Publisher image_representation_pub_AA_frequency_; // AA频率图发布器 + image_transport::Publisher image_representation_pub_AA_mat_; // AA矩阵发布器 + + bool left_; + cv::Mat negative_TS_img; + cv_bridge::CvImage cv_dx_image, cv_dy_image; + std::thread thread_sobel; + + // online parameters + bool bCamInfoAvailable_;// 相机信息是否可用 + bool bUse_Sim_Time_; // 是否使用仿真时间 + cv::Size sensor_size_; // 传感器尺寸 + ros::Time sync_time_; // 同步时间 + bool bSensorInitialized_; // 传感器是否初始化 + + + // offline parameters TODO + double decay_ms_; // 衰减时间(毫秒) + bool ignore_polarity_; // 是否忽略极性 + int median_blur_kernel_size_; // 中值滤波核大小 + int blur_size_; // 模糊核大小 + int max_event_queue_length_; // 最大事件队列长度 + int events_maintained_size_; // 维护的事件数量 + + // containers + EventQueue events_; // 事件队列 + std::vector vEvents_; // 事件向量 + cv::Mat representation_TS_; // 时间表面表示 + cv::Mat representation_AA_; // AA表示 + Eigen::MatrixXd TS_temp_map; // 临时时间表面映射 + + // for rectify + cv::Mat undistmap1_, undistmap2_; + bool is_left_, bcreat_; + + // thread mutex + std::mutex data_mutex_; // 数据互斥锁 + + enum RepresentationMode + { + Linear_TS, // 0 + AA2, // 1 + Fast // 2 + } representation_mode_; + + // parameters + bool bUseStereoCam_; + double decay_sec_; // TS param + int generation_rate_hz_; + int x_patches_, y_patches_; + // std::vector::iterator ptr_e_; + + // calib info + std::string calibInfoDir_; + std::vector trapezoid_; + }; +} // namespace image_representation +#endif // image_representation_H_ +``` 函数:`void ImageRepresentation::createImageRepresentationAtTime(const ros::Time &external_sync_time)`