fastlio lidar补偿模块的理解

基本原理

在上面的sync_packages代码中，已经拿到了符合要求的imu数据和lidar数据，现在，就基于这些数据对lidar点进行补偿，目的是补偿到结束时刻的lidar坐标系下。

坐标系介绍

总共有3个坐标系：
全局坐标系（world）、imu坐标系（body）、lidar坐标系

全局坐标系一般以开机点为原点；
imu坐标系时刻在变化，是一个纯粹以imu来推理的坐标系；
lidar坐标系：如果lidar与imu是刚性连接的，那么lidar坐标系与imu坐标系的转换关系就是固定的，知道了imu坐标系就可以知道lidar坐标系。

基本处理流程

1、imu预积分

把堆积的imu数据进行预积分处理，推导出每一个imu数据时刻相对与全局坐标系的T，以及对应的方差。
有个细节要注意：
每次开始之前，都会把上一次预积分的最后一个imu加入到本次imu队列的头部，以及对应的姿态结果作为此次预积分的开始姿态。
可以想象，这种推理会导致imu推理的轨迹飞的很快，但是不影响我们总的结果，因为我们取的是短时间内的相对运动姿态。

另外，对于此次处理的点云数据，也按照相对时间顺序进行了升序排列。
这里的

这里预积分用的是中值积分：

数据准备：

/*** add the imu of the last frame-tail to the of current frame-head ***/
  auto v_imu = meas.imu;
  v_imu.push_front(last_imu_);//让imu的时间能包住lidar的时间
  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;
  
  /*** sort point clouds by offset time ***/
  pcl_out = *(meas.lidar);
  std::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);
  std::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<<std::endl;

  /*** Initialize IMU pose ***/
  IMUpose.clear();
  // IMUpose.push_back(set_pose6d(0.0, Zero3d, Zero3d, state.vel_end, state.pos_end, state.rot_end));
  IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, state_inout.vel_end, state_inout.pos_end, state_inout.rot_end));

这是点云的排序函数：

const bool time_list(PointType &x, PointType &y) {
   
   return (x.curvature < y.curvature);};

预积分过程

典型的：

/*** forward propagation at each imu point ***/
  Eigen::Vector3d acc_imu, angvel_avr, acc_avr, vel_imu(state_inout.vel_end), pos_imu(state_inout.pos_end);
  Eigen::Matrix3d R_imu(state_inout.rot_end);
  Eigen::MatrixXd F_x(Eigen::Matrix<double, DIM_OF_STATES, DIM_OF_STATES>::Identity());
  Eigen::MatrixXd cov_w(Eigen::Matrix<double, DIM_OF_STATES, DIM_OF_STATES>::Zero());
  double dt = 0;
  for (auto it_imu = v_imu.begin(); it_imu != (v_imu.end() - 1); it_imu++)
  {
   
   //中值积分
    auto &&head = *(it_imu);
    auto &&tail = *(it_imu + 1);
    
    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 *