UPDATE

imu.cpp

+#include "preprocess.h"
+
+#define RETURN0     0x00
+#define RETURN0AND1 0x10
+
+Preprocess::Preprocess()
+  :feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
+{
+  inf_bound = 10;
+  N_SCANS   = 6;
+  group_size = 8;
+  disA = 0.01;
+  disA = 0.1; // B?
+  p2l_ratio = 225;
+  limit_maxmid =6.25;
+  limit_midmin =6.25;
+  limit_maxmin = 3.24;
+  jump_up_limit = 170.0;
+  jump_down_limit = 8.0;
+  cos160 = 160.0;
+  edgea = 2;
+  edgeb = 0.1;
+  smallp_intersect = 172.5;
+  smallp_ratio = 1.2;
+  given_offset_time = false;
+
+  jump_up_limit = cos(jump_up_limit/180*M_PI);
+  jump_down_limit = cos(jump_down_limit/180*M_PI);
+  cos160 = cos(cos160/180*M_PI);
+  smallp_intersect = cos(smallp_intersect/180*M_PI);
+}
+//这段代码定义了Preprocess类的构造函数Preprocess()，该函数初始化了类的各个成员变量。其中：
+
+   // feature_enabled：是否启用特征提取，默认为0。
+   // lidar_type：激光雷达类型，默认为AVIA。
+   // blind：盲区大小，默认为0.01。
+  //  point_filter_num：点云滤波数量，默认为1。
+   // inf_bound：最大边界值，默认为10。
+    //N_SCANS：激光雷达扫描线数量，默认为6。
+   // group_size：一组点云包含的点数，默认为8。
+   // disA：两组激光束之间的距离，默认为0.01。
+   // p2l_ratio：点到线距离与点到点距离的比值，默认为225。
+   // limit_maxmid：最大中间距离，默认为6.25。
+   // limit_midmin：中间距离和最小距离的比值，默认为6.25。
+   // limit_maxmin：最大距离和最小距离的比值，默认为3.24。
+   // jump_up_limit：上跳阈值，默认为170.0度。
+   // jump_down_limit：下跳阈值，默认为8.0度。
+   // cos160：cos(160.0度)的值，默认为0.94。
+  //  edgea：边缘判断参数A，默认为2。
+   // edgeb：边缘判断参数B，默认为0.1。
+    //smallp_intersect：小角度交叉判断参数，默认为cos(172.5度)。
+  //  smallp_ratio：小角度交叉判断参数，默认为1.2。
+   //given_offset_time：是否给定位移时间，默认为false。
+
+//接下来，代码对 jump_up_limit、jump_down_limit、cos160和smallp_intersect 进行了一些计算，将角度转换为弧度并计算其余弦值，以便于后续使用。
+//总体来说，这段代码主要是对类的成员变量进行初始化，并进行了少量的计算和赋值//
+// Preprocess类的析构函数
+Preprocess::~Preprocess() {}
+
+// 设置预处理参数的函数
+void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
+{
+  feature_enabled = feat_en; //启用/禁用特征提取
+  lidar_type = lid_type; //使用的LiDAR传感器类型
+  blind = bld; //删除距离超出一定范围之外的点的距离阈值
+  point_filter_num = pfilt_num; //单次过滤需要过滤的点数
+}
+
+// 处理LiDAR点云数据的函数
+void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
+{  
+  avia_handler(msg); //将Avia滤波器应用于LiDAR点云数据
+  *pcl_out = pl_surf; //将过滤后的点云赋值给输出点云
+}
+
+void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
+{
+  switch (lidar_type)  // 根据激光雷达类型选择处理函数
+  {
+  case OUST64:
+    oust64_handler(msg);  // 处理 Ouster OS1-64 的点云数据
+    break;
+
+  case VELO16:
+    velodyne_handler(msg);  // 处理 Velodyne VLP-16 的点云数据
+    break;
+  
+  default:
+    printf("Error LiDAR Type");  // 如果输入的激光雷达类型无法识别，则打印错误信息
+    break;
+  }
+  *pcl_out = pl_surf;  // 将处理后的点云数据赋值给输出参数
+}
+
+void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
+{
+  pl_surf.clear();    //清空表面点云
+  pl_corn.clear();    //清空角点点云
+  pl_full.clear();    //清空完整点云
+  double t1 = omp_get_wtime();    //获取当前时间
+  int plsize = msg->point_num;    //获取点云数据数量
+
+  pl_corn.reserve(plsize);    //预留角点点云空间
+  pl_surf.reserve(plsize);    //预留表面点云空间
+  pl_full.resize(plsize);    //设置完整点云大小为点云数据数量
+
+  for(int i=0; i<N_SCANS; i++)    //循环遍历激光雷达扫描数次
+  {
+    pl_buff[i].clear();    //清空每次扫描的点云缓存
+    pl_buff[i].reserve(plsize);    //预留每次扫描点云缓存的空间
+  }
+  uint valid_num = 0;    //有效点云数量初始化为0
+
+  if (feature_enabled)    //判断特征是否可用
+  {
+    for(uint i=1; i<plsize; i++)    //循环遍历点云数据
+    {
+      if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))    //判断点的线数与点类型是否符合条件
+      {
+        pl_full[i].x = msg->points[i].x;    //获取点的x坐标
+        pl_full[i].y = msg->points[i].y;    //获取点的y坐标
+        pl_full[i].z = msg->points[i].z;    //获取点的z坐标
+        pl_full[i].intensity = msg->points[i].reflectivity;    //获取点的反射强度
+        pl_full[i].curvature = msg->points[i].offset_time / float(1000000);    //获取点的偏移时间并将其作为曲率值
+
+        bool is_new = false;    //初始化点是否为新的变量为false
+        if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7)    //判断点的x,y,z坐标与上一个点是否相同
+            || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
+            || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7))
+        {
+          pl_buff[msg->points[i].line].push_back(pl_full[i]);    //将点加入点云缓存
+        }
+      }
+    } 
+
+   // 静态变量，记录运行次数和时间
+static int count = 0;
+static double time = 0.0;
+count++;
+
+// 获取当前时间
+double t0 = omp_get_wtime();
+
+// 循环进行点云特征提取
+for(int j=0; j<N_SCANS; j++)
+{
+  // 如果点云数量小于等于5，则跳过本次循环
+  if(pl_buff[j].size() <= 5) continue;
+
+  // 获取当前循环中的点云和类型
+  pcl::PointCloud<PointType> &pl = pl_buff[j];
+  plsize = pl.size();
+  vector<orgtype> &types = typess[j];
+  types.clear();
+  types.resize(plsize);
+  plsize--;
+
+  // 对于点云中每个点分别计算它的range和dista
+  for(uint i=0; i<plsize; i++)
+  {
+    types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y;
+    vx = pl[i].x - pl[i+1].x;
+    vy = pl[i].y - pl[i+1].y;
+    vz = pl[i].z - pl[i+1].z;
+    types[i].dista = vx * vx + vy *vy + vz * vz;
+  }
+
+  // 计算最后一个点的range，并进行特征提取
+  types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y;
+  give_feature(pl, types);
+  // pl_surf = pl;
+}
+
+// 计算特征提取的时间并打印出来
+time = omp_get_wtime() - t0;
+printf("Feature extraction time: %lf \n", time / count);
+
+// 如果不需要进行点云特征提取，则将点云中的有效点提取到pl_surf点云序列中
+else
+{
+  // 遍历所有点，判断是否为有效点
+  for(uint i=1; i<plsize; i++)
+  {
+    if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
+    {
+      valid_num++;
+
+      // 如果满足筛选条件，则记录为有效点
+      if (valid_num % point_filter_num == 0)
+      {
+        pl_full[i].x = msg->points[i].x;
+        pl_full[i].y = msg->points[i].y;
+        pl_full[i].z = msg->points[i].z;
+        pl_full[i].intensity = msg->points[i].reflectivity;
+        pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points
+
+        // 判断是否为遮挡区域，如果不是则记录在pl_surf点云序列中
+        if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) 
+            || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
+            || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)
+            && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y > blind))
+        {
+          pl_surf.push_back(pl_full[i]);
+        }
+      }
+    }
+  }
+}
+
+
+void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
+{
+  pl_surf.clear();   // 清表面点云
+  pl_corn.clear();   // 清除角点点云
+  pl_full.clear();   // 清除完整点云
+  pcl::PointCloud<ouster_ros::Point> pl_orig;   // 定义 ouster_ros::Point 类型的点云
+  pcl::fromROSMsg(*msg, pl_orig);   // 将 ROS 消息转换成 PCL 点云格式
+  int plsize = pl_orig.size();   // 获取点云的数量
+  pl_corn.reserve(plsize);   // 为角点点云预留空间
+  pl_surf.reserve(plsize);   // 为表面点云预留空间
+  if (feature_enabled)   // 如果开启了特征点提取
+  {
+    for (int i = 0; i < N_SCANS; i++)   // 遍历所有扫描线
+    {
+      pl_buff[i].clear();   // 清空扫描线点云
+      pl_buff[i].reserve(plsize);   // 为扫描线点云预留空间
+    }
+
+    for (uint i = 0; i < plsize; i++)   // 遍历所有点云
+    {
+      double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z;   // 计算点云距离原点的距离
+      if (range < blind) continue;   // 如果距离小于盲区跳过
+      Eigen::Vector3d pt_vec;   // 定义一个三维向量
+      PointType added_pt;   // 定义一个加入点云的点
+      added_pt.x = pl_orig.points[i].x;   // 记录点云的 x 坐标
+      added_pt.y = pl_orig.points[i].y;   // 记录点云的 y 坐标
+      added_pt.z = pl_orig.points[i].z;   // 记录点云的 z 坐标
+      added_pt.intensity = pl_orig.points[i].intensity;   // 记录点云的强度值
+      added_pt.normal_x = 0;   // 记录点云的法向量 x 坐标（初始值为 0）
+      added_pt.normal_y = 0;   // 记录点云的法向量 y 坐标（初始值为 0）
+      added_pt.normal_z = 0;   // 记录点云的法向量 z 坐标（初始值为 0）
+      double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;   // 计算点云的方位角（角度制）
+      if (yaw_angle >= 180.0)
+        yaw_angle -= 360.0;
+      if (yaw_angle <= -180.0)
+        yaw_angle += 360.0;
+
+      added_pt.curvature = pl_orig.points[i].t / 1e6;   // 记录点云的曲率
+      if(pl_orig.points[i].ring < N_SCANS)   // 如果点云在扫描范围内
+      {
+        pl_buff[pl_orig.points[i].ring].push_back(added_pt);   // 将点云加入对应的扫描线点云中
+      }
+    }
+
+    for (int j = 0; j < N_SCANS; j++)   // 遍历所有扫描线
+    {
+      PointCloudXYZI &pl = pl_buff[j];   // 获取当前扫描线点云
+      int linesize = pl.size();   // 获取扫描线点云的数量
+      vector<orgtype> &types = typess[j];   // 获取当前扫描线的特征点类型
+      types.clear();   // 清空特征点类型
+      types.resize(linesize);   // 为特征点类型预留空间
+      linesize--;
+      for (uint i = 0; i < linesize; i++)   // 遍历扫描线点云
+      {
+        types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);   // 计算点云距离原点的距离
+        vx = pl[i].x - pl[i+1].x;   // 计算点云的横向距离
+        vy = pl[i].y - pl[i+1].y;   // 计算点云的纵向距离
+        vz = pl[i].z - pl[i+1].z;   // 计算点云的垂直距离
+        types[i].dista = vx * vx + vy * vy + vz * vz;   // 计算点云之间的距离
+      }
+      types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);   // 处理最后一个点云的距离
+      give_feature(pl, types);   // 将扫描线点云的特征提取出来
+    }
+  }
+  else   // 如果未开启特征点提取
+  {
+    double time_stamp = msg->header.stamp.toSec();   // 获取消息的时间戳
+
+   // 循环遍历原始点云中的每个点
+for (int i = 0; i < pl_orig.points.size(); i++) {
+
+  // 如果当前点的索引不是筛选倍数的整数，则跳过
+  if (i % point_filter_num != 0) continue;
+
+  // 计算当前点到原点的距离
+  double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z;
+
+  // 如果当前点距离太小，则跳过
+  if (range < blind) continue;
+
+  // 将当前点的坐标、强度、曲率信息保存到新点云中
+  PointType added_pt;
+  added_pt.x = pl_orig.points[i].x;
+  added_pt.y = pl_orig.points[i].y;
+  added_pt.z = pl_orig.points[i].z;
+  added_pt.intensity = pl_orig.points[i].intensity;
+  added_pt.curvature = pl_orig.points[i].t / 1e6;
+
+  // 将当前点的法向量信息设为 0
+  added_pt.normal_x = 0;
+  added_pt.normal_y = 0;
+  added_pt.normal_z = 0;
+
+  // 计算当前点的偏航角
+  double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
+  if (yaw_angle >= 180.0) yaw_angle -= 360.0;
+  if (yaw_angle <= -180.0) yaw_angle = 360.0;
+
+  // 将新点添加到新点云中
+  pl_surf.points.push_back(added_pt);
+}
+
+  // pub_func(pl_surf, pub_full, msg->header.stamp);
+  // pub_func(pl_surf, pub_corn, msg->header.stamp);
+}
+
+// 定义点云数量的最大值
+#define MAX_LINE_NUM 64
+
+void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
+{
+    // 清空三个点云容器
+    pl_surf.clear();   
+    pl_corn.clear();   
+    pl_full.clear();   
+
+    // 将输入的ros消息类型的点云转换为pcl中的点云
+    pcl::PointCloud<velodyne_ros::Point> pl_orig;
+    pcl::fromROSMsg(*msg, pl_orig);
+    int plsize = pl_orig.points.size();
+    pl_surf.reserve(plsize);
+
+    bool is_first[MAX_LINE_NUM];   // 标记每行数据的第一个点
+    double yaw_fp[MAX_LINE_NUM]={0};     // 存储每行数据的第一个点的偏航角
+    double omega_l=3.61;       // Velodyne激光雷达扫描的相对角速率
+    float yaw_last[MAX_LINE_NUM]={0.0};  // 存储每行数据的最后一个点的偏航角
+    float time_last[MAX_LINE_NUM]={0.0}; // 存储最后一个点时间戳
+
+    // 判断点云数据是否存在时间戳
+    if (pl_orig.points[plsize - 1].time > 0)
+    {
+      given_offset_time = true;
+    }
+    else
+    {
+      given_offset_time = false;
+      memset(is_first, true, sizeof(is_first));
+      double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;  // 计算第一个点的偏航角
+      double yaw_end  = yaw_first;
+      int layer_first = pl_orig.points[0].ring;  // 获取第一层的激光线数量
+      for (uint i = plsize - 1; i > 0; i--)
+      {
+        if (pl_orig.points[i].ring == layer_first)   // 找到最后一层的激光线数量
+        {
+          yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;   // 计算最后一个点的偏航角
+          break;
+        }
+      }
+    } 
+
+
+if(feature_enabled)  //检查特征是否已启用
+{
+  for (int i = 0; i < N_SCANS; i++)  //循环遍历每一个激光雷达扫描
+  {
+    pl_buff[i].clear();  //清空激光雷达缓冲区
+    pl_buff[i].reserve(plsize);  //为激光雷达缓冲区预留空间
+  }
+  
+  for (int i = 0; i < plsize; i++)  //循环遍历激光雷达点云的每一个点
+  {
+    PointType added_pt;  //定义一个点类型
+    added_pt.normal_x = 0;  //将该点的法线向量赋值为0
+    added_pt.normal_y = 0;
+    added_pt.normal_z = 0;
+    int layer  = pl_orig.points[i].ring;  //获取该点的环号（对应于扫描线数目）
+    if (layer >= N_SCANS) continue;  //如果环号大于等于扫描线数目，则跳过该点
+    added_pt.x = pl_orig.points[i].x;  //将该点的坐标赋值给定义的点类型
+    added_pt.y = pl_orig.points[i].y;
+    added_pt.z = pl_orig.points[i].z;
+    added_pt.intensity = pl_orig.points[i].intensity;  //将该点的强度值赋值给定义的点类型
+    added_pt.curvature = pl_orig.points[i].time / 1000.0; //将该点的曲率值赋值给定义的点类型，单位为ms
+
+
+
+       if (!given_offset_time) // 如果偏移时间为空
+{
+    // 计算航向角
+    double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+    
+    if (is_first[layer]) // 如果当前层为第一次处理
+    {
+        // 将当前航向角作为起始航向角
+        yaw_fp[layer] = yaw_angle;
+        // 将is_first[layer]设置为false
+        is_first[layer] = false;
+        // 将曲率设为0
+        added_pt.curvature = 0.0;
+        // 更新最后一次处理的航向角(yaw_last)为当前航向角(yaw_angle)
+        yaw_last[layer] = yaw_angle;
+        // 更新最后一次处理的曲率(time_last)为当前曲率(added_pt.curvature)
+        time_last[layer] = added_pt.curvature;
+        // 跳过当前循环
+        continue;
+    } 
+}
+
+
+        if (yaw_angle <= yaw_fp[layer])  //如果当前航向角小于等于目标航向角
+{
+  added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l;  //计算运动点的曲率
+}
+else //如果当前航向角大于目标航向角
+{
+  added_pt.curvature = (yaw_fp[layer]-yaw_angle 360.0) / omega_l;  //计算运动点的曲率
+}
+
+if (added_pt.curvature < time_last[layer])  added_pt.curvature =360.0/omega_l;  //如果曲率小于上一个点的曲率，则赋值为最小曲率，即直线
+
+yaw_last[layer] = yaw_angle;  //记录航向角作为上一个点的航向角
+time_last[layer]=added_pt.curvature;  //记录曲率作为上一个点的曲率
+
+
+    pl_buff[layer].points.push_back(added_pt); //将添加的点加入点云缓存中的第layer层
+
+for (int j = 0; j < N_SCANS; j++ ) //循环遍历点云中的每一层
+{
+  PointCloudXYZI &pl = pl_buff[j]; //获取第j层点云
+  int linesize = pl.size(); //获取该层点云的点数
+  if (linesize < 2) continue; //如果该层点云的点数小于2，则跳过该层
+  vector<orgtype> &types = typess[j]; //获取该层点云的特征类型
+  types.clear(); //清空特征类型向量
+  types.resize(linesize); //将特征类型向量大小设置为该层点云的点数
+  linesize--; //将点数减1，以便后续计算
+
+  //遍历该层点云的每个点，计算特征类型
+  for (uint i = 0; i < linesize; i++ )
+  {
+    types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); //计算该点到雷达的距离
+    vx = pl[i].x - pl[i+1].x; //计算该点与下一个点在x方向上的距离差
+    vy = pl[i].y - pl[i+1].y; //计算该点与下一个点在y方向上的距离差
+    vz = pl[i].z - pl[i+1].z; //计算该点与下一个点在z方向上的距离差
+    types[i].dista = vx * vx + vy * vy + vz * vz; //计算该点与下一个点之间的距离
+  }
+  types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); //计算该层点云最后一个点到雷达的距离
+  give_feature(pl, types); //对该层点云进行特征提取
+}
+
+   else
+{
+  // 循环遍历plsize个点
+  for (int i = 0; i < plsize; i++) 
+  {
+    PointType added_pt;
+    // 初始化added_pt的normal和坐标
+    added_pt.normal_x = 0;
+    added_pt.normal_y = 0;
+    added_pt.normal_z = 0;
+    added_pt.x = pl_orig.points[i].x;
+    added_pt.y = pl_orig.points[i].y;
+    added_pt.z = pl_orig.points[i].z;
+    added_pt.intensity = pl_orig.points[i].intensity;
+    // added_pt的曲率属性为时间值除以1000
+    added_pt.curvature = pl_orig.points[i].time / 1000.0;
+
+    // 如果没有给定时间偏移，进入以下操作
+    if (!given_offset_time) 
+    {
+      // 获取点所在的激光环编号和偏航角度
+      int layer = pl_orig.points[i].ring;
+      double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+
+      // 如果该激光环是第一个点，更新yaw_fp、is_first，设置曲率属性为0，跳过该点
+      if (is_first[layer])
+      {
+          yaw_fp[layer]=yaw_angle;
+          is_first[layer]=false;
+          added_pt.curvature = 0.0;
+          yaw_last[layer]=yaw_angle;
+          time_last[layer]=added_pt.curvature;
+          continue; 
+
+          }
+
+         // 计算偏移时间
+if (yaw_angle <= yaw_fp[layer]) // 如果当前的偏航角小于等于目标偏航角
+{
+  added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; // 偏移时间为目标偏航角减去当前偏航角的值再除以角速度omega_l
+}
+else // 如果当前偏航角大于目标偏航角
+{
+  added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; // 偏移时间为目标偏航角减去360度再加上当前偏航角的值再除以角速度omega_l
+}
+
+if (added_pt.curvature < time_last[layer]) added_pt.curvature = 360.0 / omega_l; // 如果偏移时间小于上一次的偏移时间，则将偏移时间设置为一个很大的值
+
+yaw_last[layer] = yaw_angle; // 更新上一次的偏航角为当前偏航角
+time_last[layer] = added_pt.curvature; // 更新上一次的偏移时间为当前偏移时间
+
+
+        // if(i==(plsize-1))  printf("index: %d layer: %d, yaw: %lf, offset-time: %lf, condition: %d\n", i, layer, yaw_angle, added_pt.curvature, prints);
+        if (i % point_filter_num == 0)
+        {
+          if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > blind)
+          {
+            pl_surf.points.push_back(added_pt);
+            // printf("time mode: %d time: %d \n", given_offset_time, pl_orig.points[i].t);
+          }
+        }
+      }
+    }
+
+    
+    // pub_func(pl_surf, pub_full, msg->header.stamp);
+    // pub_func(pl_surf, pub_surf, msg->header.stamp);
+    // pub_func(pl_surf, pub_corn, msg->header.stamp);
+}
+
+void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types)
+{
+  int plsize = pl.size();
+  int plsize2;
+  if(plsize == 0)
+  {
+    printf("something wrong\n");
+    return;
+  }
+  uint head = 0;
+
+  while(types[head].range < blind)
+  {
+    head++;
+  }
+
+  // Surf
+  // 计算剩余IMU数据点数
+plsize2 = (plsize > group_size) ? (plsize - group_size) : 0;
+
+// 定义当前方向向量和上一个方向向量
+Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero());
+Eigen::Vector3d last_direct(Eigen::Vector3d::Zero());
+
+// 定义两个索引变量和最后一个有效索引变量，以及一个状态和平面类型
+uint i_nex = 0, i2;
+uint last_i = 0; 
+uint last_i_nex = 0;
+int last_state = 0;
+int plane_type;
+
+// 循环处理IMU数据
+for(uint i=head; i<plsize2; i++)
+{
+  // 如果数据点的距离小于盲区距离，则跳过当前循环，不进行处理
+  if(types[i].range < blind)
+  {
+    continue;
+  } 
+
+
+    i2 = i;
+
+  // 判断当前路径的飞机类型
+plane_type = plane_judge(pl, types, i, i_nex, curr_direct);
+    
+if(plane_type == 1)  // 如果为确立路径，则进行以下操作
+{
+  for(uint j=i; j<=i_nex; j++)  // 遍历路径中所有点
+  { 
+    if(j!=i && j!=i_nex)
+    {
+      types[j].ftype = Real_Plane;  // 非起点和终点的点标记为实际路径点
+    }
+    else
+    {
+      types[j].ftype = Poss_Plane;  // 起点和终点标记为可能路径点
+    }
+  } 
+} 
+
+      
+      // if(last_state==1 && fabs(last_direct.sum())>0.5)
+ // 如果上一个平面状态为1且上一个方向向量的模大于0.1
+if (last_state == 1 && last_direct.norm() > 0.1)
+{
+  // 计算上一个方向向量和当前方向向量的点积
+  double mod = last_direct.transpose() * curr_direct;
+  
+  // 如果点积的值在-0.707到0.707之间，将边缘平面的类型设为Edge_Plane
+  if (mod > -0.707 && mod < 0.707)
+  {
+    types[i].ftype = Edge_Plane;
+  }
+  // 否则将其设为Real_Plane
+  else
+  {
+    types[i].ftype = Real_Plane;
+  }
+}
+
+// 将计数器移动到下一个索引位置并相应地更新上一个平面状态
+i = i_nex - 1;
+last_state = 1;
+
+// 如果平面类型为2，将计数器移动到下一个索引位置并将上一个状态设为0
+else
+{
+  i = i_nex;
+  last_state = 0;
+} 
+
+    // else if(plane_type == 0)
+    // {
+    //   if(last_state == 1)
+    //   {
+    //     uint i_nex_tem;
+    //     uint j;
+    //     for(j=last_i+1; j<=last_i_nex; j++)
+    //     {
+    //       uint i_nex_tem2 = i_nex_tem;
+    //       Eigen::Vector3d curr_direct2;
+
+    //       uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2);
+
+    //       if(ttem != 1)
+    //       {
+    //         i_nex_tem = i_nex_tem2;
+    //         break;
+    //       }
+    //       curr_direct = curr_direct2;
+    //     }
+
+    //     if(j == last_i+1)
+    //     {
+    //       last_state = 0;
+    //     }
+    //     else
+    //     {
+    //       for(uint k=last_i_nex; k<=i_nex_tem; k++)
+    //       {
+    //         if(k != i_nex_tem)
+    //         {
+    //           types[k].ftype = Real_Plane;
+    //         }
+    //         else
+    //         {
+    //           types[k].ftype = Poss_Plane;
+    //         }
+    //       }
+    //       i = i_nex_tem-1;
+    //       i_nex = i_nex_tem;
+    //       i2 = j-1;
+    //       last_state = 1;
+    //     }
+
+    //   }
+    // }
+
+   // 记录上个平面的信息
+last_i = i2;
+last_i_nex = i_nex;
+last_direct = curr_direct;
+
+// 计算平面局部法向量
+plsize2 = plsize > 3 ? plsize - 3 : 0;
+for(uint i=head+3; i<plsize2; i++)    // 从第4个点开始计算
+{
+  if(types[i].range<blind || types[i].ftype>=Real_Plane)    // 排除无效点或非地面点
+  {
+    continue;
+  }
+
+  if(types[i-1].dista<1e-16 || types[i].dista<1e-16)    // 排除距离过小的点
+  {
+    continue;
+  }
+
+  // 计算当前点和相邻两个点的向量
+  Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z);
+  Eigen::Vector3d vecs[2];
+
+  for(int j=0; j<2; j++)    // 对于当前点的前一个点和后一个点
+  {
+    int m = -1;
+    if(j == 1)
+    {
+      m = 1;
+    }
+
+    // 如果相邻的点无效，则直接标记为不可连接
+    if(types[i + m].range < blind)
+    {
+      if(types[i].range > inf_bound)
+      {
+        types[i].edj[j] = Nr_inf;
+      }
+      else
+      {
+        types[i].edj[j] = Nr_blind;
+      }
+      continue;
+    }
+
+    // 计算向量并计算夹角
+    vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z);
+    vecs[j] = vecs[j] - vec_a;
+      
+    types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm();
+
+    // 标记当前点和相邻点之间的连接类型
+    if(types[i].angle[j] < jump_up_limit)
+    {
+      types[i].edj[j] = Nr_180;
+    }
+    else if(types[i].angle[j] > jump_down_limit)
+    {
+      types[i].edj[j] = Nr_zero;
+    }
+  } 
+}
+
+
+  // 计算两个向量之间的交点
+types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();
+
+// 检查当前边缘是否是一个潜在的边缘跳跃，通过比较边的类型和距离来确定
+if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista)
+{
+  // 检查交点角度是否大于160度
+  if(types[i].intersect > cos160)
+  {
+    // 如果确认了边缘跳跃，请检查上一个边缘的类型是否正常
+    if(edge_jump_judge(pl, types, i, Prev))
+    {
+      // 将边的类型设置为Edge_Jump
+      types[i].ftype = Edge_Jump;
+    }
+  }
+}
+// 检查当前边缘是否是一个潜在的边缘跳跃，通过比较边的类型和距离来确定
+else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista)
+{
+  // 检查交点角度是否大于160度
+  if(types[i].intersect > cos160)
+  {
+    // 如果确认了边缘跳跃，请检查下一个边缘的类型是否正常
+    if(edge_jump_judge(pl, types, i, Next))
+    {
+      // 将边的类型设置为Edge_Jump
+      types[i].ftype = Edge_Jump;
+    }
+  }
+} 
+
+  //检查前一个边缘是不是普通边缘且下一个边缘是无穷边缘
+else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf)
+{
+  //如果检测到边缘跳变，将ftype设置为Edge_Jump
+  if(edge_jump_judge(pl, types, i, Prev))
+  {
+    types[i].ftype = Edge_Jump;
+  }
+}
+
+//检查前一个边缘是不是无穷边缘且下一个边缘是普通边缘
+else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor)
+{
+  //如果检测到边缘跳变，将ftype设置为Edge_Jump
+  if(edge_jump_judge(pl, types, i, Next))
+  {
+    types[i].ftype = Edge_Jump;
+  }
+}
+
+//检查前一个边缘和下一个边缘都不是普通边缘
+else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor)
+{
+  //如果当前ftype为Nor，则将其设置为Wire
+  if(types[i].ftype == Nor)
+  {
+    types[i].ftype = Wire;
+  }
+}
+
+
+  // 将 plsize 减 1 的结果赋值给 plsize2 变量
+plsize2 = plsize - 1;
+// 声明变量 ratio 为 double 类型
+double ratio;
+// 使用 for 循环遍历一个范围的值，从头部 + 1 的位置到 plsize2 的位置
+for (uint i = head + 1; i < plsize2; i++)
+{
+  // 检查当前点和它相邻点的 range 值是否小于 blind 值，如果是，则跳过当前循环，继续下一次循环
+  if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind)
+  {
+    continue;
+  }
+  // 检查当前点和它前一个点的 dista 值是否小于 1e-8，如果是，则跳过当前循环，继续下一次循环
+  if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8)
+  {
+    continue;
+  }
+  // 检查当前点的 feature type 是否为 Nor
+  if (types[i].ftype == Nor)
+  {
+    // 如果前一个点的 dista 值大于当前点的 dista 值，则将 ratio 值赋值为前一个点的 dista 值除以当前点的 dista 值；否则将 ratio 值赋值为当前点的 dista 值除以前一个点的 dista 值
+    if (types[i - 1].dista > types[i].dista)
+    {
+      ratio = types[i - 1].dista / types[i].dista;
+    }
+    else
+    {
+      ratio = types[i].dista / types[i - 1].dista;
+    }
+    // 如果当前点和其相邻点的 feature type 均为 Nor，且当前点的 intersect 值小于 smallp_intersect，且 ratio 值小于 smallp_ratio，将这些点的 feature type 均修改为 Real_Plane
+    if (types[i].intersect < smallp_intersect && ratio < smallp_ratio)
+    {
+      if (types[i - 1].ftype == Nor)
+      {
+        types[i - 1].ftype = Real_Plane;
+      }
+      if (types[i + 1].ftype == Nor)
+      {
+        types[i + 1].ftype = Real_Plane;
+      }
+      types[i].ftype = Real_Plane;
+    }
+  }
+}
+
+ // 初始化last_surface为-1
+int last_surface = -1;
+
+// 遍历点云数据
+for(uint j = head; j < plsize; j++)
+{
+
+    // 如果当前点类型是平面类型（Possible_Plane 或 Real_Plane）
+    if(types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane)
+    {
+        // 如果上一个点不是平面，则记录当前点为第一个平面点
+        if(last_surface == -1)
+        {
+            last_surface = j;
+        }
+
+        // 判断当前的点是否为上一个平面点的下一个点
+        if(j == uint(last_surface + point_filter_num - 1))
+        {
+            // 如果是，则将该平面点加入到pl_surf数组中
+            PointType ap;
+            ap.x = pl[j].x;
+            ap.y = pl[j].y;
+            ap.z = pl[j].z;
+            ap.intensity = pl[j].intensity;
+            ap.curvature = pl[j].curvature;
+            pl_surf.push_back(ap);
+
+            // 重置last_surface为-1，表示当前平面已经处理完毕
+            last_surface = -1;
+        }
+    }
+    else
+    {
+        // 如果当前点类型不是平面类型，且上一个点是平面类型，说明当前点是边角点
+        if(types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane)
+        {
+            // 将该点加入到pl_corn数组中
+            pl_corn.push_back(pl[j]);
+        }
+
+        // 如果上一个点是平面类型，则将该平面点的平均值加入到pl_surf数组中
+        if(last_surface != -1)
+        {
+            PointType ap;
+            for(uint k = last_surface; k < j; k++)
+            {
+                ap.x  += pl[k].x;
+                ap.y  += pl[k].y;
+                ap.z  += pl[k].z;
+                ap.intensity  += pl[k].intensity;
+                ap.curvature  += pl[k].curvature;
+            }
+            ap.x /= (j - last_surface);
+            ap.y /= (j - last_surface);
+            ap.z /= (j - last_surface);
+            ap.intensity /= (j - last_surface);
+            ap.curvature /= (j - last_surface);
+            pl_surf.push_back(ap);
+        }
+
+        // 重置last_surface为-1
+        last_surface = -1;
+    }
+}
+
+
+// 这是一个名为Preprocess的类中的pub_func方法，用于将PointCloudXYZI类型的点云数据转换为sensor_msgs::PointCloud2类型的数据并发布出去
+void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct)
+{
+  // 将点云的高度设置为1，宽度设置为点云的大小
+  pl.height = 1; pl.width = pl.size();
+  // 将点云数据转换为ROS消息类型
+  sensor_msgs::PointCloud2 output;
+  pcl::toROSMsg(pl, output);
+  // 设置ROS消息的帧id为“livox”，时间戳为当前时间
+  output.header.frame_id = "livox";
+  output.header.stamp = ct;
+}
+
+
+int Preprocess::plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct)
+{
+  double group_dis = disA*types[i_cur].range - disB;  // 计算检测范围的距离
+
+  group_dis = group_dis * group_dis;
+  // i_nex = i_cur;
+
+  double two_dis;
+  vector<double> disarr;
+  disarr.reserve(20);
+
+  for(i_nex=i_cur; i_nex<i_cur + group_size; i_nex++)  // 选取当前点为起始点，向后取group_size个点进行处理
+  {
+    if(types[i_nex].range < blind)  // 如果范围小于盲区，则无法进行处理
+    {
+      curr_direct.setZero();  // 清零向量
+      return 2;
+    }
+    disarr.push_back(types[i_nex].dista);  // 将距离值添加到列表中
+  } 
+
+  
+  // 无限循环，直到满足条件跳出
+for(;;)
+{
+	// 如果索引超出数组大小则跳出循环
+	if((i_cur >= pl.size()) || (i_nex >= pl.size())) break;
+
+	// 判断下一点的距离是否小于盲区距离
+	if(types[i_nex].range < blind)
+	{
+		curr_direct.setZero();
+		return 2; // 返回2
+	}
+
+	// 计算下一点与当前点的距离
+	vx = pl[i_nex].x - pl[i_cur].x;
+	vy = pl[i_nex].y - pl[i_cur].y;
+	vz = pl[i_nex].z - pl[i_cur].z;
+	two_dis = vx*vx + vy*vy + vz*vz;
+
+	// 如果当前距离大于等于组距，则退出循环
+	if(two_dis >= group_dis)
+	{
+		break;
+	}
+
+	// 如果当前距离小于组距，则将当前点的特征距离存入disarr中，并将下一点索引+1
+	disarr.push_back(types[i_nex].dista);
+	i_nex ++;
+}
+
+// 初始化长度和宽度为0
+double leng_wid = 0;
+double v1[3], v2[3];
+
+// 循环遍历当前点与下一点之间的所有点
+for(uint j=i_cur+1; j<i_nex; j++)
+{
+	// 如果索引超出数组大小则跳出循环
+	if((j >= pl.size()) || (i_cur >= pl.size())) break;
+
+	// 计算当前点与下一点的向量
+	v1[0] = pl[j].x - pl[i_cur].x;
+	v1[1] = pl[j].y - pl[i_cur].y;
+	v1[2] = pl[j].z - pl[i_cur].z;
+
+	// 计算当前点与下一点的法向量
+	v2[0] = v1[1]*vz - vy*v1[2];
+	v2[1] = v1[2]*vx - v1[0]*vz;
+	v2[2] = v1[0]*vy - vx*v1[1];
+
+	// 计算长度和宽度的平方
+	double lw = v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2];
+
+	// 如果当前长度和宽度的平方大于之前记录的，则更新记录
+	if(lw > leng_wid)
+	{
+		leng_wid = lw;
+	}
+} 
+
+
+
+  // 如果两个轮子的距离除以车辆长度小于轮胎宽度和长度的比值，则设置当前方向为零向量并返回0
+if((two_dis*two_dis/leng_wid) < p2l_ratio)
+{
+  curr_direct.setZero();
+  return 0;
+}
+
+// 对车辆四个轮子到地面的距离进行从大到小的排序
+uint disarrsize = disarr.size();
+for(uint j=0; j<disarrsize-1; j++)
+{
+  for(uint k=j+1; k<disarrsize; k++)
+  {
+    if(disarr[j] < disarr[k]) // 如果第j个距离小于第k个距离，交换位置，使距离从大到小排序
+    {
+      leng_wid = disarr[j];
+      disarr[j] = disarr[k];
+      disarr[k] = leng_wid;
+    }
+  }
+}
+
+ // 判断倒数第二个距离值是否小于 1e-16，如果是，将 curr_direct 设为 0 向量，然后返回 0。
+if(disarr[disarr.size()-2] < 1e-16)
+{
+  curr_direct.setZero();
+  return 0;
+}
+
+// 判断激光雷达类型是否为 AVIA。
+if(lidar_type==AVIA)
+{
+  // 计算 dismax_mid 和 dismid_min。
+  double dismax_mid = disarr[0]/disarr[disarrsize/2];
+  double dismid_min = disarr[disarrsize/2]/disarr[disarrsize-2];
+
+  // 如果 dismax_mid 大于等于 limit_maxmid 或者 dismid_min 大于等于 limit_midmin，将 curr_direct 设为 0 向量，然后返回 0。
+  if(dismax_mid>=limit_maxmid || dismid_min>=limit_midmin)
+  {
+    curr_direct.setZero();
+    return 0;
+  }
+}
+else // 如果激光雷达类型不是 AVIA。
+{
+  // 计算 dismax_min。
+  double dismax_min = disarr[0] / disarr[disarrsize-2];
+
+  // 如果 dismax_min 大于等于 limit_maxmin，将 curr_direct 设为 0 向量，然后返回 0。
+  if(dismax_min >= limit_maxmin)
+  {
+    curr_direct.setZero();
+    return 0;
+  }
+}
+
+// 将 vx、vy、vz 组成向量 curr_direct。
+curr_direct << vx, vy, vz;
+
+// 将 curr_direct 归一化。
+curr_direct.normalize();
+
+// 返回 1。
+return 1;
+
+
+bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir)
+{
+  // 判断法向量的方向
+  if(nor_dir == 0)
+  {
+    //如果该点和前两个点中有任意一个点的激光雷达测量范围小于盲区，则返回false
+    if(types[i-1].range<blind || types[i-2].range<blind)
+    {
+      return false;
+    }
+  }
+  else if(nor_dir == 1)
+  {
+    //如果该点和后两个点中有任意一个点的激光雷达测量范围小于盲区，则返回false
+    if(types[i + 1].range<blind || types[i + 2].range<blind)
+    {
+      return false;
+    }
+  } 
+
+// 获取朝向的索引值，用于计算距离
+double d1 = types[i nor_dir-1].dista;
+double d2 = types[i 3*nor_dir-2].dista;
+double d;
+
+// 如果d1小于d2，则交换d1和d2的值
+if(d1<d2)
+{
+  d = d1;
+  d1 = d2;
+  d2 = d;
+}
+
+// 计算两点之间的距离
+d1 = sqrt(d1);
+d2 = sqrt(d2);
+
+// 判断是否在边界内，如果在边界内则返回true，否则返回false
+if(d1>edgea*d2 || (d1-d2)>edgeb)
+{
+  return false;
+}
+
+return true;

imu222.cpp

+#include <cmath>
+#include <math.h>
+#include <deque>
+#include <mutex>
+#include <thread>
+#include <fstream>
+#include <csignal>
+#include <ros/ros.h>
+#include <so3_math.h>
+#include <Eigen/Eigen>
+#include <common_lib.h>
+#include <pcl/common/io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <condition_variable>
+#include <nav_msgs/Odometry.h>
+#include <pcl/common/transforms.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <tf/transform_broadcaster.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/Vector3.h>
+#include "use-ikfom.hpp"
+
+/// *************Preconfiguration
+
+/**
+ * 定义 MAX_INI_COUNT 常量为 20
+ */
+#define MAX_INI_COUNT (20)
+
+/**
+ * 比较两个 PointType 类型的对象的曲率值，按升序排列
+ * @param x PointType 类型的对象
+ * @param y PointType 类型的对象
+ * @return 返回比较结果，如果 x 的曲率小于 y 的曲率，返回 true，否则返回 false
+ */
+const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; 
+
+
+/// *************IMU Process and undistortion
+class ImuProcess
+{
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  ImuProcess();  // 构造函数
+  ~ImuProcess();  // 析构函数
+  
+  void Reset();  // 复位函数
+  void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu);  // 复位函数，带有起始时间戳和上一个IMU的指针参数
+  void set_extrinsic(const V3D &transl, const M3D &rot);  // 设置外参，包括平移和旋转矩阵
+  void set_extrinsic(const V3D &transl);  // 设置外参，仅包括平移
+  void set_extrinsic(const MD(4,4) &T);  // 设置外参，包括齐次变换矩阵
+  void set_gyr_cov(const V3D &scaler);  // 设置角速度测量噪声的方差
+  void set_acc_cov(const V3D &scaler);  // 设置加速度测量噪声的方差
+  void set_gyr_bias_cov(const V3D &b_g);  // 设置角速度Bias噪声的方差
+  void set_acc_bias_cov(const V3D &b_a);  // 设置加速度Bias噪声的方差
+  Eigen::Matrix<double, 12, 12> Q;  // 12x12的协方差矩阵
+  void Process(const MeasureGroup &meas,  esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr pcl_un_);  // IMU处理函数
+
+  ofstream fout_imu;  // 输出文件流
+  V3D cov_acc;  // 加速度噪声方差
+  V3D cov_gyr;  // 角速度噪声方差
+  V3D cov_acc_scale;  // 加速度scale噪声方差
+  V3D cov_gyr_scale;  // 角速度scale噪声方差
+  V3D cov_bias_gyr;  // 角速度Bias噪声方差
+  V3D cov_bias_acc;  // 加速度Bias噪声方差
+  double first_lidar_time;  // 第一个激光雷达时间戳
+
+
+ // 声明私有成员函数
+private:
+  // IMU初始化函数
+  void IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N);
+  // 去除点云畸变函数
+  void UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_in_out);
+  
+  // 点云数据成员指针
+  PointCloudXYZI::Ptr cur_pcl_un_;
+  // 上一个IMU数据成员指针
+  sensor_msgs::ImuConstPtr last_imu_;
+  // IMU数据成员双端队列
+  deque<sensor_msgs::ImuConstPtr> v_imu_;
+  // IMU位姿数据存储向量
+  vector<Pose6D> IMUpose;
+  // 点云旋转矩阵存储向量
+  vector<M3D>    v_rot_pcl_;
+  // 相对于IMU坐标系的激光雷达旋转矩阵
+  M3D Lidar_R_wrt_IMU;
+  // 相对于IMU坐标系的激光雷达位置向量
+  V3D Lidar_T_wrt_IMU;
+  // IMU加速度均值向量
+  V3D mean_acc;
+  // IMU陀螺仪均值向量
+  V3D mean_gyr;
+  // 上一个时刻的角速度向量
+  V3D angvel_last;
+  // 上一个时刻的加速度向量
+  V3D acc_s_last;
+  // 初始时间戳
+  double start_timestamp_;
+  // 上一个激光雷达结束时间
+  double last_lidar_end_time_;
+  // 初始化迭代次数
+  int init_iter_num = 1;
+  // 是否是第一帧数据
+  bool b_first_frame_ = true;
+  // 是否需要IMU初始化
+  bool imu_need_init_ = true;
+}; 
+
+
+ImuProcess::ImuProcess()  //定义ImuProcess的构造函数
+    : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1) //成员变量初始化
+{
+  init_iter_num = 1;  //初始化参数
+  Q = process_noise_cov(); //初始化噪声矩阵Q
+  cov_acc = V3D(0.1, 0.1, 0.1); //初始化加速度计噪声协方差
+  cov_gyr = V3D(0.1, 0.1, 0.1); //初始化陀螺仪噪声协方差
+  cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); //初始化陀螺仪偏置噪声协方差
+  cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); //初始化加速度计偏置噪声协方差
+  mean_acc = V3D(0, 0, -1.0); //初始化加速度计均值
+  mean_gyr = V3D(0, 0, 0); //初始化陀螺仪均值
+  angvel_last = Zero3d; //设置上一个角速度为零向量
+  Lidar_T_wrt_IMU = Zero3d; //设置Lidar_T_wrt_IMU为零向量
+  Lidar_R_wrt_IMU = Eye3d; //设置Lidar_R_wrt_IMU为单位矩阵
+  last_imu_.reset(new sensor_msgs::Imu()); //将last_imu_初始化为一个新的sensor_msgs::Imu指针
+} 
+
+ImuProcess::~ImuProcess() {} //析构函数，释放对象时调用
+
+void ImuProcess::Reset() 
+{
+  // ROS_WARN("Reset ImuProcess"); //注释掉的代码，不会被执行，ROS_WARN是输出警告信息的函数
+  mean_acc      = V3D(0, 0, -1.0); //将均值加速度重置为(0, 0, -1.0)
+  mean_gyr      = V3D(0, 0, 0);    //将均值陀螺仪重置为(0, 0, 0)
+  angvel_last       = Zero3d;  //将上一个角速度值重置为0
+  imu_need_init_    = true;    //需要初始化IMU
+  start_timestamp_  = -1;      //起始时间戳为-1
+  init_iter_num     = 1;       //初始化迭代次数为1
+  v_imu_.clear();              //清空imu数据
+  IMUpose.clear();             //清空imu位姿数据
+  last_imu_.reset(new sensor_msgs::Imu()); //将上一个IMU数据重置为新的sensor_msgs::Imu对象
+  cur_pcl_un_.reset(new PointCloudXYZI()); //将当前点云数据重置为新的PointCloudXYZI对象
+}
+
+
+void ImuProcess::set_extrinsic(const MD(4,4) &T)
+{
+  // 设置激光雷达相对于IMU的平移向量
+  Lidar_T_wrt_IMU = T.block<3,1>(0,3);
+  // 设置激光雷达相对于IMU的旋转矩阵
+  Lidar_R_wrt_IMU = T.block<3,3>(0,0);
+}
+
+void ImuProcess::set_extrinsic(const V3D &transl)
+{
+  // 设置激光雷达相对于IMU的平移向量
+  Lidar_T_wrt_IMU = transl;
+  // 设置激光雷达相对于IMU的旋转矩阵为单位矩阵
+  Lidar_R_wrt_IMU.setIdentity();
+}
+
+void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot)
+{
+  // 设置激光雷达相对于IMU的平移向量
+  Lidar_T_wrt_IMU = transl;
+  // 设置激光雷达相对于IMU的旋转矩阵
+  Lidar_R_wrt_IMU = rot;
+}
+
+void ImuProcess::set_gyr_cov(const V3D &scaler)
+{
+  // 设置陀螺仪的测量噪声方差
+  cov_gyr_scale = scaler;
+} 
+
+
+// 设置加速度计噪声协方差矩阵的比例因子
+void ImuProcess::set_acc_cov(const V3D &scaler)
+{
+  cov_acc_scale = scaler;
+}
+
+// 设置陀螺仪零偏噪声协方差矩阵
+void ImuProcess::set_gyr_bias_cov(const V3D &b_g)
+{
+  cov_bias_gyr = b_g;
+}
+
+// 设置加速度计零偏噪声协方差矩阵
+void ImuProcess::set_acc_bias_cov(const V3D &b_a)
+{
+  cov_bias_acc = b_a;
+}
+
+// IMU初始化函数
+void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N) 
+
+{
+  /** 1. initializing the gravity, gyro bias, acc and gyro covariance
+   ** 2. normalize the acceleration measurenments to unit gravity **/
+  
+  V3D cur_acc, cur_gyr;
+  
+ if (b_first_frame_) // 如果是第一帧数据
+{
+  // 进行初始化
+  Reset(); // 重置一些变量
+  N = 1; // 记录数据数量
+  b_first_frame_ = false; // 标记为不是第一帧数据
+  const auto &imu_acc = meas.imu.front()->linear_acceleration; // 获取第一帧imu数据的加速度
+  const auto &gyr_acc = meas.imu.front()->angular_velocity; // 获取第一帧imu数据的角速度
+  mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; // 计算加速度均值
+  mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; // 计算角速度均值
+  first_lidar_time = meas.lidar_beg_time; // 记录第一帧激光雷达数据的时间戳
+}
+
+
+  // 循环遍历测量中的每个IMU数据
+for (const auto &imu : meas.imu)
+{
+  // 从IMU数据中提取线性加速度和角速度
+  const auto &imu_acc = imu->linear_acceleration;
+  const auto &gyr_acc = imu->angular_velocity;
+
+  // 将当前加速度和角速度保存在Eigen向量中
+  cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
+  cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
+
+  // 计算平均加速度和角速度
+  mean_acc       = (cur_acc - mean_acc) / N;
+  mean_gyr       = (cur_gyr - mean_gyr) / N;
+
+  // 计算加速度和角速度的协方差矩阵
+  cov_acc = cov_acc * (N - 1.0) / N   (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N);
+  cov_gyr = cov_gyr * (N - 1.0) / N   (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N);
+
+  // 增加已处理的测量数量
+  N++;
+}
+
+// 从卡尔曼滤波器状态获取初始化状态
+state_ikfom init_state = kf_state.get_x();
+
+// 根据平均加速度计算重力向量并设置为初始化状态
+init_state.grav = S2(-mean_acc / mean_acc.norm() * G_m_s2);
+
+// 获取当前IMU的方向
+const auto& orientation = meas.imu.front()->orientation;
+
+// 如果orientation消息可用，则使用imu的orientation作为初始姿态
+if( orientation.x != 0 || orientation.y != 0 || orientation.z != 0 || orientation.w != 0 ){
+  Eigen::Quaterniond qua(orientation.w, orientation.x, orientation.y, orientation.z);
+  init_state.rot = qua; 
+}
+
+  }
+  // 初始化状态量
+//state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity))); 
+// 上述语句是注释掉的，意为旋转状态初始化为单位矩阵，即不旋转，但后续也没有使用到，因此可以忽略
+init_state.bg  = mean_gyr;  // 陀螺仪偏置初始化为零偏
+init_state.offset_T_L_I = Lidar_T_wrt_IMU;  // 激光雷达相对IMU的位置初始化
+init_state.offset_R_L_I = Lidar_R_wrt_IMU;  // 激光雷达相对IMU的旋转初始化
+kf_state.change_x(init_state);  // 更新KF的状态量
+
+// 初始化协方差矩阵
+esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P();  // 获取KF的协方差矩阵
+init_P.setIdentity();  // 设置为对角线全为1的单位矩阵
+init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001;  // 陀螺仪偏置协方差初始化为0.00001
+init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001;  // 激光雷达位置偏差协方差初始化为0.00001
+init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001;  // 速度偏差协方差初始化为0.0001
+init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001;  // 位置偏差协方差初始化为0.001
+init_P(21,21) = init_P(22,22) = 0.00001;  // 重力加速度偏差协方差初始化为0.00001
+kf_state.change_P(init_P);  // 更新KF的协方差矩阵
+
+last_imu_ = meas.imu.back();  // 记录最后一个IMU数据，用于后续计算时间间隔
+
+
+}
+
+// 通过对接收到的IMU和当前EKF状态信息来去畸变点云数据。
+
+void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out)
+{
+  // 将上一帧IMU数据的尾部添加到当前帧IMU数据的头部。
+  auto v_imu = meas.imu;
+  v_imu.push_front(last_imu_);
+
+  // 获取IMU和点云数据的时间戳
+  const double &imu_beg_time = v_imu.front()->header.stamp.toSec();
+  const double &imu_end_time = v_imu.back()->header.stamp.toSec();
+  const double &pcl_beg_time = meas.lidar_beg_time;
+
+  // 根据时间排序点云数据
+  pcl_out = *(meas.lidar);
+  sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
+  const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000); 
+}
+
+  // cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
+  //          <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<endl;
+
+  /*** Initialize IMU pose ***/
+  state_ikfom imu_state = kf_state.get_x();
+  IMUpose.clear();
+  IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
+
+  /*** forward propagation at each imu point ***/
+  V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;
+  M3D R_imu;
+
+  double dt = 0;
+
+  input_ikfom in;
+  for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)
+  {
+    auto &&head = *(it_imu);
+    auto &&tail = *(it_imu + 1);
+    
+    if (tail->header.stamp.toSec() < last_lidar_end_time_)    continue;
+    
+    angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
+                0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
+                0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
+    acc_avr   <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
+                0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
+                0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);
+
+    fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
+
+    acc_avr     = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;
+
+    if(head->header.stamp.toSec() < last_lidar_end_time_)
+    {
+      dt = tail->header.stamp.toSec() - last_lidar_end_time_;
+      // dt = tail->header.stamp.toSec() - pcl_beg_time;
+    }
+    else
+    {
+      dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
+    }
+    
+   // 将平均角速度以及平均加速度设置为Kalman滤波器的输入
+in.acc = acc_avr;
+in.gyro = angvel_avr;
+
+// 将协方差矩阵的对角线设置为陀螺仪和加速度计的方差，以及陀螺仪偏移和加速度计偏移的方差
+Q.block<3, 3>(0, 0).diagonal() = cov_gyr;
+Q.block<3, 3>(3, 3).diagonal() = cov_acc;
+Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
+Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
+
+// 使用Kalman滤波器的predict函数进行预测，传入时间步长dt，协方差矩阵Q以及输入in
+kf_state.predict(dt, Q, in); 
+
+
+    /* save the poses at each IMU measurements */
+  // 获取当前时刻的KF状态量
+imu_state = kf_state.get_x();
+
+// 计算当前时刻的角速度
+angvel_last = angvel_avr - imu_state.bg;
+
+// 计算当前时刻的加速度
+acc_s_last = imu_state.rot * (acc_avr - imu_state.ba);
+
+// 将加速度调整到重力加速度方向
+for(int i=0; i<3; i++ )
+{
+    acc_s_last[i] = imu_state.grav[i];
+}
+
+// 计算时间偏移量、更新IMUpose向量
+double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time;
+IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
+
+  /*** calculated the pos and attitude prediction at the frame-end ***/
+ // 确定note，基于pcl_end_time和imu_end_time的比较。note用于决定在预测步骤中加或减pcl_end_time和imu_end_time之间的时间差
+double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;
+// 计算pcl_end_time和imu_end_time之间的时间差
+dt = note * (pcl_end_time - imu_end_time);
+// 使用时间差和输入噪声协方差矩阵预测卡尔曼滤波器的状态
+kf_state.predict(dt, Q, in);
+
+// 更新imu_state为当前卡尔曼滤波器的状态
+imu_state = kf_state.get_x();
+// 存储最后一个IMU测量值
+last_imu_ = meas.imu.back();
+// 存储点云的最后一个时间戳
+last_lidar_end_time_ = pcl_end_time;
+
+/*** 反向传播，使每个激光点失真 ***/
+
+// 初始化迭代器到最后一个IMU位姿和点云中的最后一个点
+auto it_pcl = pcl_out.points.end() - 1;
+for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
+{
+  // 将head设置为it_kp之前的IMU位姿，将tail设置为it_kp
+  auto head = it_kp - 1;
+  auto tail = it_kp;
+  // 从head IMU位姿中提取旋转矩阵
+  R_imu<<MAT_FROM_ARRAY(head->rot);
+  // 从head IMU位姿中提取速度向量
+  vel_imu<<VEC_FROM_ARRAY(head->vel);
+  // 从head IMU位姿中提取位置向量
+  pos_imu<<VEC_FROM_ARRAY(head->pos);
+  // 从tail IMU位姿中提取加速度向量
+  acc_imu<<VEC_FROM_ARRAY(tail->acc);
+  // 从tail IMU位姿中提取角速度向量
+  angvel_avr<<VEC_FROM_ARRAY(tail->gyr);
+
+  // 遍历所有曲率大于head IMU位姿的偏移时间的激光点
+  for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--)
+  {
+    // 计算head IMU位姿和激光点之间的时间差
+    dt = it_pcl->curvature / double(1000) - head->offset_time;
+
+
+      /* Transform to the 'end' frame, using only the rotation
+       * Note: Compensation direction is INVERSE of Frame's moving direction
+       * So if we want to compensate a point at timestamp-i to the frame-e
+       * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */
+      M3D R_i(R_imu * Exp(angvel_avr, dt));
+      
+      V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
+      V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
+      V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
+      
+      // save Undistorted points and their rotation
+      it_pcl->x = P_compensate(0);
+      it_pcl->y = P_compensate(1);
+      it_pcl->z = P_compensate(2);
+
+      if (it_pcl == pcl_out.points.begin()) break;
+    }
+  }
+}
+
+void ImuProcess::Process(const MeasureGroup &meas,  esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr cur_pcl_un_)
+{
+  double t1,t2,t3;
+  t1 = omp_get_wtime();
+
+  if(meas.imu.empty()) {return;};
+  ROS_ASSERT(meas.lidar != nullptr);
+
+  if (imu_need_init_)
+  {
+    /// The very first lidar frame
+    IMU_init(meas, kf_state, init_iter_num);
+
+    imu_need_init_ = true;
+    
+    last_imu_   = meas.imu.back();
+
+    state_ikfom imu_state = kf_state.get_x();
+    if (init_iter_num > MAX_INI_COUNT)
+    {
+      cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
+      imu_need_init_ = false;
+
+      cov_acc = cov_acc_scale;
+      cov_gyr = cov_gyr_scale;
+      ROS_INFO("IMU Initial Done");
+      // ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
+      //          imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
+      fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out);
+    }
+
+    return;
+  }
+
+  UndistortPcl(meas, kf_state, *cur_pcl_un_);
+
+  t2 = omp_get_wtime();
+  t3 = omp_get_wtime();
+  
+  // cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;
+}
\ No newline at end of file