UPDATE

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