UPDATE

imu222.cpp

@@ -420,64 +420,97 @@ for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
     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 */
+      /* 使用旋转将点转换为最终坐标系
+       * 注意：补偿方向与坐标系移动方向相反。
+       * 如果我们要将时间戳-i的点补偿到end帧，则
+       * P_compensate = R_imu_e ^ T *（R_i * P_i T_ei），其中T_ei在全局坐标系中表示 */
+
+      // 计算从当前IMU状态到“end”帧的旋转矩阵
       M3D R_i(R_imu * Exp(angvel_avr, dt));
       
+      // 将点坐标存储在一个3D向量中
       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
+      // 计算从当前IMU状态到“end”帧的平移向量，用全局坐标系表示
+      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);
+      
+      // 将补偿点和它们的旋转重新存储在点云中
       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;
+      // 如果当前点是点云中的第一个点，则跳出循环
+      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();
+  double t1,t2,t3; //定义三个双精度浮点数变量t1，t2，t3用于计时
+  t1 = omp_get_wtime(); //记录开始时间
 
-  if(meas.imu.empty()) {return;};
-  ROS_ASSERT(meas.lidar != nullptr);
+  if(meas.imu.empty()) {return;}; //如果IMU数据为空，则返回
 
-  if (imu_need_init_)
-  {
-    /// The very first lidar frame
-    IMU_init(meas, kf_state, init_iter_num);
+  ROS_ASSERT(meas.lidar != nullptr); //使用ROS的断言函数，确保LIDAR数据不为空
 
-    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);
-    }
+ if (imu_need_init_)
+{
+  /// 第一个激光雷达帧
+
+  // 如果需要初始化IMU
+  // 这是第一个激光雷达帧
+
+  IMU_init(meas, kf_state, init_iter_num);
+
+  // 调用IMU初始化函数
+
+  imu_need_init_ = true;
 
-    return;
+  // 设置标志以指示IMU尚未初始化
+
+  last_imu_   = meas.imu.back();
+
+  // 获得最新的IMU测量
+
+  state_ikfom imu_state = kf_state.get_x();
+
+  // 从卡尔曼滤波器获取IMU的当前状态
+
+  if (init_iter_num > MAX_INI_COUNT)
+  {
+    cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
+    imu_need_init_ = false;
+
+    // 更新加速度计和陀螺仪测量的协方差
+    // 设置标志以指示IMU已初始化
+    // 迭代次数足够，IMU初始化完成
+
+    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);
   }
 
-  UndistortPcl(meas, kf_state, *cur_pcl_un_);
+  // 将加速度计和陀螺仪测量的协方差更新为默认值
+  // 打印一条消息，说明IMU初始化已完成
+  // 打开IMU调试文件以进行写入
 
-  t2 = omp_get_wtime();
-  t3 = omp_get_wtime();
-  
-  // cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;
-}
\ No newline at end of file
+  return;
+}
+
+  // 调用UndistortPcl函数对点云进行去畸变处理
+UndistortPcl(meas, kf_state, *cur_pcl_un_);
+
+t2 = omp_get_wtime(); // 记录程序运行时间
+t3 = omp_get_wtime(); 
+
+// 输出程序运行时间
+// cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;

头文件.cpp

+#// 包含所需的头文件
+#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)
+)
+
+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,
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (float, intensity, intensity)
+    // use std::uint32_t to avoid conflicting with pcl::uint32_t
+    (std::uint32_t, t, t)
+    (std::uint16_t, reflectivity, reflectivity)
+    (std::uint8_t, ring, ring)
+    (std::uint16_t, ambient, ambient)
+    (std::uint32_t, range, 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]; //maximum 128 line lidar
+  vector<orgtype> typess[128]; //maximum 128 line lidar
+  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);
+  void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+  void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+  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;
+  double edgea, edgeb;
+  double smallp_intersect, smallp_ratio;
+  double vx, vy, vz;
+};
\ No newline at end of file