From db492f0498b8ce124c66f187b7d976fff2d37728 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Tue, 25 Mar 2025 12:03:44 +0800 Subject: [PATCH] feat: Add enumerations and comments for lidar processing structures --- src/preprocess.h | 73 +++++++++++++++++++++++++++--------------------- 1 file changed, 41 insertions(+), 32 deletions(-) diff --git a/src/preprocess.h b/src/preprocess.h index 35f3833..687f52f 100644 --- a/src/preprocess.h +++ b/src/preprocess.h @@ -14,20 +14,26 @@ using namespace std; typedef pcl::PointXYZINormal PointType; typedef pcl::PointCloud PointCloudXYZI; +// 激光雷达类型枚举 enum LID_TYPE{AVIA = 1, VELO16, OUST64, MARSIM}; //{1, 2, 3} +// 时间单位枚举 enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3}; +// 特征类型枚举 enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; +// 周围点枚举 enum Surround{Prev, Next}; +// 边缘跳跃类型枚举 enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; +// 原始类型结构体 struct orgtype { - double range; - double dista; - double angle[2]; - double intersect; - E_jump edj[2]; - Feature ftype; + double range; // 距离 + double dista; // 距离差 + double angle[2]; // 角度 + double intersect; // 交点 + E_jump edj[2]; // 边缘跳跃类型 + Feature ftype; // 特征类型 orgtype() { range = 0; @@ -41,9 +47,9 @@ struct orgtype namespace velodyne_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; - float intensity; - float time; - uint16_t ring; + float intensity; // 强度 + float time; // 时间 + uint16_t ring; // 环 EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace velodyne_ros @@ -59,12 +65,12 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, namespace ouster_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; - float intensity; - uint32_t t; - uint16_t reflectivity; - uint8_t ring; - uint16_t ambient; - uint32_t range; + float intensity; // 强度 + uint32_t t; // 时间 + uint16_t reflectivity; // 反射率 + uint8_t ring; // 环 + uint16_t ambient; // 环境光 + uint32_t range; // 距离 EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace ouster_ros @@ -90,23 +96,26 @@ class Preprocess Preprocess(); ~Preprocess(); - + + // 处理函数 void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); void set(bool feat_en, int lid_type, double bld, int pfilt_num); // sensor_msgs::PointCloud2::ConstPtr pointcloud; + // 点云数据 PointCloudXYZI pl_full, pl_corn, pl_surf; - PointCloudXYZI pl_buff[128]; //maximum 128 line lidar - vector typess[128]; //maximum 128 line lidar - float time_unit_scale; - int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; - double blind; - bool feature_enabled, given_offset_time; - ros::Publisher pub_full, pub_surf, pub_corn; + PointCloudXYZI pl_buff[128]; // 最大128线激光雷达 + vector typess[128]; // 最大128线激光雷达 + float time_unit_scale; // 时间单位比例 + int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; // 激光雷达类型、点过滤数量、扫描次数、扫描速率、时间单位 + double blind; // 盲区 + bool feature_enabled, given_offset_time; // 特征使能、给定偏移时间 + ros::Publisher pub_full, pub_surf, pub_corn; // 发布器 private: + // 处理函数 void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); @@ -117,14 +126,14 @@ class Preprocess bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); - int group_size; - double disA, disB, inf_bound; - double limit_maxmid, limit_midmin, limit_maxmin; - double p2l_ratio; - double jump_up_limit, jump_down_limit; - double cos160; - double edgea, edgeb; - double smallp_intersect, smallp_ratio; - double vx, vy, vz; + int group_size; // 组大小 + double disA, disB, inf_bound; // 距离参数、无穷边界 + double limit_maxmid, limit_midmin, limit_maxmin; // 限制参数 + double p2l_ratio; // 比例 + double jump_up_limit, jump_down_limit; // 跳跃限制 + double cos160; // 余弦值 + double edgea, edgeb; // 边缘参数 + double smallp_intersect, smallp_ratio; // 小平面参数 + double vx, vy, vz; // 向量 }; #endif \ No newline at end of file