preprocess注释.h

#// 包含所需的头文件
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver/CustomMsg.h>

using namespace std;

// 定义点云类型和点云指针类型
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;

// 定义枚举类型，表示不同型号的激光雷达和不同的特征类型等
enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 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}; 

// 宏定义，用于判断激光点坐标是否合法
#define IS_VALID(a)  ((abs(a)>1e8) ? true : false)


// 定义了一个结构体 orgtype（车辆感知系统中的障碍物类型），包含以下成员变量：
// range：障碍物到车辆的距离
// dista：车辆到障碍物的距离
// angle：车辆与障碍物的夹角，包括左右两个角度
// intersect：障碍物与车辆轨迹的交点数目，默认为2
// edj：障碍物前后两个点的换道预测值（E_jump类型），默认为Nr_nor
// ftype：障碍物类型，包括Normal、Pedestrian、Cyclist等，默认为Nor
struct orgtype
{
    double range;
    double dista;
    double angle[2];
    double intersect;
    E_jump edj[2];
    Feature ftype;

    // 构造函数，初始值为0或默认值
    orgtype()
    {
        range = 0;
        edj[Prev] = Nr_nor;
        edj[Next] = Nr_nor;
        ftype = Nor;
        intersect = 2;
    }
}; 


namespace velodyne_ros {
  struct EIGEN_ALIGN16 Point {
      PCL_ADD_POINT4D;
      float intensity;
      float time;
      uint16_t ring;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
}  // namespace velodyne_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
    (float, time, time)
    (uint16_t, ring, ring)
)
//这段代码定义了一个名为velodyne_ros::Point的结构体，表示了Velodyne激光雷达采集到的点云数据，包括点云的三维坐标(x, y, z)、强度(intensity)、时间戳(time)、环号(ring)等信息。其中，PCL_ADD_POINT4D用于为结构体添加四元素坐标系，uint16_t表示无符号16位整数，EIGEN_ALIGN16和EIGEN_MAKE_ALIGNED_OPERATOR_NEW是为了保证内存对齐。最后，POINT_CLOUD_REGISTER_POINT_STRUCT宏用于在PCL(Point Cloud Library)中注册结构体，使其可以被PCL识别和使用。
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;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
}  // namespace ouster_ros

// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,  // 注册ouster_ros命名空间下的Point结构体
    (float, x, x)               // 定义x坐标域，类型为float，名称为x
    (float, y, y)               // 定义y坐标域，类型为float，名称为y
    (float, z, z)               // 定义z坐标域，类型为float，名称为z
    (float, intensity, intensity)  // 定义强度域，类型为float，名称为intensity
    // 使用std::uint32_t避免与pcl命名空间下的uint32_t冲突
    (std::uint32_t, t, t)       // 定义时间戳域，类型为std::uint32_t，名称为t
    (std::uint16_t, reflectivity, reflectivity)  // 定义反射率域，类型为std::uint16_t，名称为reflectivity
    (std::uint8_t, ring, ring)  // 定义激光环号域，类型为std::uint8_t，名称为ring
    (std::uint16_t, ambient, ambient)  // 定义环境光域，类型为std::uint16_t，名称为ambient
    (std::uint32_t, range, range)  // 定义距离域，类型为std::uint32_t，名称为range
) 


class Preprocess
{
  public:
//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 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]; // 定义点云缓存数组（最大支持128线激光雷达）
vector<orgtype> typess[128]; // 定义激光雷达点云类型数组（最大支持128线激光雷达）
int lidar_type, point_filter_num, N_SCANS; // 定义激光雷达类型、点云过滤阈值、扫描线数目
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);  // 处理Livox Avia类型雷达数据的回调函数
  void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);  // 处理Velodyne Ouster64型号激光雷达数据的回调函数
  void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);  // 处理Velodyne VLP-16型号激光雷达数据的回调函数
  void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);  // 提取点云中的特征点
  void pub_func(PointCloudXYZI &pl, const ros::Time &ct);  // 发布处理后的点云的函数
  int  plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);  // 判断平面点的函数
  bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);  // 判断小平面点的函数
  bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &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;  // 平面点判定时的角度cos值阈值
  double edgea, edgeb;  // 边缘点判定时的距离阈值与角度阈值
  double smallp_intersect, smallp_ratio;  // 小平面点判定时的交点距离阈值与占比阈值
  double vx, vy, vz;  // 相对运动的速度