关于lidar用imu进行运动补偿的细节

利用imu在短时间100ms内的运动，补偿雷达的运动畸变，只进行旋转方向的补偿。

1、找配对数据
以lidar为基准，找到包含lidar一帧的点的起始、结束时刻的imu数据。

2、对这些imu数据，进行积分处理
以第一个imu为基准坐标，依次积分所有的imu的数据，每个imu都得到一个对应pose。

3、计算结束时刻的lidar的相对于第一个imu的姿态
lidar不一定和imu的时间对齐，因此，要进行插值。
找到包含这个结束时刻的最近的左边、右边的imu，得到其姿态和时间。
然后利用插值的方法，插值出这个lidar时刻的姿态，注意，这个姿态是imu坐标系的，要转换成lidar坐标系相对于第一个imu坐标系的姿态。

4、将整个点云的点按照第3步处理方式，计算出其lidar坐标系相对于第一个imu的姿态，再转换到最后一个时刻的lidar坐标系下。

5、速度的考虑
1）考虑到点云很多，几万个点，没有必要每个点的时刻都计算一次姿态，将100ms，划分成10个段，计算10个段的姿态，落在某一个段内的，就用对应的姿态进行计算。
2）并行化
这些处理的逻辑是简单的，且是无相关的，可以用omp的方式进行加速处理

6、之前参考的lidar补偿的一些问题
参考的livox的运动补偿，是用时间比例进行插值的方式进行补偿，这个处理，在lidar与imu有平移量的时候，是不能精准插值的。

7、代码

流程代码：

void ImuProcess::Process(const MeasureGroup &meas) {
   
   
  if(meas.imu.empty() || meas.lidar==nullptr){
   
   
    return;
  }
  // ROS_ASSERT(!meas.imu.empty());
  // ROS_ASSERT(meas.lidar != nullptr);
  // ROS_DEBUG("Process lidar at time: %.4f, %lu imu msgs from %.4f to %.4f",
  //           meas.lidar->header.stamp.toSec(), meas.imu.size(),
  //           meas.imu.front()->header.stamp.toSec(),
  //           meas.imu.back()->header.stamp.toSec());

  auto pcl_in_msg = meas.lidar;

  /**
  if (b_first_frame_) {
    /// The very first lidar frame

    /// Reset
    // Reset();

    /// Record first lidar, and first useful imu
    last_lidar_ = pcl_in_msg;
    last_imu_ = meas.imu.back();

    ROS_WARN("The very first lidar frame");

    /// Do nothing more, return
    b_first_frame_ = false;
    return;
  }
  */

  /// Integrate all input imu message
  Reset();

  pcl::fromROSMsg(*pcl_in_msg, *cur_pcl_in_);

  double lidar_beg_time=cur_pcl_in_->points[0].timestamp;
  double lidar_end_time=cur_pcl_in_->points.back().timestamp;
  double imu_beg_time=meas.imu.front()->header.stamp.toSec();
  double imu_end_time=meas.imu.back()->header.stamp.toSec();
  //找到包含lidar begin and lidar end的两个imu
  uint imu_idx_for_lidar_beg_1=0;
  uint imu_idx_for_lidar_beg_2=0;
  uint imu_idx_for_lidar_end_1=0;
  uint imu_idx_for_lidar_end_2=0;

  IntegrateGyr(meas.imu,
  lidar_beg_time,
  lidar_end_time,
  imu_idx_for_lidar_beg_1,
  imu_idx_for_lidar_beg_2,
  imu_idx_for_lidar_end_1,
  imu_idx_for_lidar_end_2
  );//积分出来的是imu最后相对于最前的变化，如果与pcl的时间不一致，还不能直接用
  
  double t_lidar_beg_to_imu_beg=lidar_beg_time-imu_beg_time;
  double t_imu_time_range=imu_end_time-imu_beg_time;
  double t_lidar_time_range=lidar_end_time-lidar_beg_time;
  double t_lidar_end_to_imu_end=imu_end_time-lidar_end_time;

  

  //求出lidar起始时刻、结束时刻的imu的姿态
  //包含此lidar的imu的时间差
  double imu_time_contain_beg_lidar=meas.imu[imu_idx_for_lidar_beg_2]->header.stamp.toSec()-meas.imu[imu_idx_for_lidar_beg_1]->header.stamp.toSec();
  //lidar start time to 包含此ldiar的开始的imu的时间
  double lidar_start_time_to_contain_imu_start=lidar_beg_time-meas.imu[imu_idx_for_lidar_beg_1]->header.stamp.toSec();
  std::vector<Sophus::SO3d> imu_poses=gyr_int_.GetRots();
  Sophus::SO3d imu_pose_1=imu_poses[imu_idx_for_lidar_beg_1];
  Sophus::SO3d imu_pose_2=imu_poses[imu_idx_for_lidar_beg_2];
  Sophus::SO3d lidar_beg_so3=GetInterpolateSO3(
  imu_pose_1,imu_pose_2,
  imu_time_contain_beg_lidar,
  lidar_start_time_to_contain_imu_start
  );

  //结束时刻的lidar姿态
  double imu_time_contain_end_lidar=meas.imu[imu_idx_for_lidar_end_2]->header.stamp.toSec()-meas.imu[imu_idx_for_lidar_end_1]->header.