0. 简介

我们刚刚了解过DLIO的整个流程，我们发现相比于Point-LIO而言，这个方法更适合我们去学习理解，同时官方给出的结果来看DLIO的结果明显好于现在的主流方法，当然指的一提的是，这个DLIO是必须需要六轴IMU的，所以如果没有IMU的画，那只有DLO可以使用了。

1. initializeDLIO—初始化参数

这段代码非常简单，主要作用是等待IMU数据的接收和校准，如果条件不满足则直接返回，否则将dlio_initialized变量设置为true表示初始化完成。其中，first_imu_received和imu_calibrated是两个布尔类型的变量，用于判断是否接收到IMU数据和是否已经校准。如果条件不满足，函数直接返回，不进行后续的初始化操作。

void dlio::OdomNode::initializeDLIO() {

  // Wait for IMU
  if (!this->first_imu_received ||
      !this->imu_calibrated) { //如果没有接收到IMU数据或者IMU没有校准
    return;
  }

  this->dlio_initialized = true; //初始化完成
  std::cout << std::endl << " DLIO initialized!" << std::endl;
}

2. getScanFromROS—ROS数据包转换与激光类型判断

这个函数也是比较简单的函数，将ROS消息类型的点云数据转换为PCL点云类型并进行预处理。函数首先将ROS消息类型的点云数据转换为PCL点云类型，并去除无效点。然后函数使用crop滤波器对点云进行框选范围内的点的筛选。接着，函数通过检测点云数据的字段来自动确定传感器类型，如果无法确定则将deskew_标志设置为false。最后，函数将原始点云数据保存到original_scan变量中，并将点云数据的时间戳保存到scan_header_stamp变量中。

void dlio::OdomNode::getScanFromROS(
    const sensor_msgs::PointCloud2ConstPtr &pc) {

  pcl::PointCloud<PointType>::Ptr original_scan_(
      boost::make_shared<pcl::PointCloud<PointType>>());
  pcl::fromROSMsg(*pc, *original_scan_);

  // 去除无效点
  std::vector<int> idx;
  original_scan_->is_dense = false;
  pcl::removeNaNFromPointCloud(*original_scan_, *original_scan_, idx);

  // crop框选范围内的点
  this->crop.setInputCloud(original_scan_);
  this->crop.filter(*original_scan_);

  // 自动检测传感器类型
  this->sensor = dlio::SensorType::UNKNOWN;
  for (auto &field : pc->fields) {
    if (field.name == "t") {
      this->sensor = dlio::SensorType::OUSTER;
      break;
    } else if (field.name == "time") {
      this->sensor = dlio::SensorType::VELODYNE; // velodyne雷达
      break;
    }
  }

  if (this->sensor == dlio::SensorType::UNKNOWN) {
    this->deskew_ = false;
  }

  this->scan_header_stamp = pc->header.stamp;
  this->original_scan = original_scan_;
}

3. preprocessPoints—-逐点去畸变与统一预积分

下面的代码对激光雷达点云数据进行预处理。首先，如果激光雷达可以逐点去畸变，将通过去畸变函数deskewPointcloud()进行处理，否则会使用统一预积分。如果是第一次处理扫描数据，需要等待IMU数据到达后才能进行处理。如果已经有了IMU数据，会通过IMU积分得到先验位姿，然后将原始点云转换到baselink坐标系下。接下来，如果开启了Voxel Grid Filter，则会对点云进行滤波处理，否则直接使用转换后的点云。最终，处理后的点云数据将作为当前激光雷达扫描的输入数据。

void dlio::OdomNode::preprocessPoints() {

  // 取消原始数据类型扫描
  if (this->deskew_) { //如果可以去畸变。也就是不为dlio::SensorType::UNKNOWN

    this->deskewPointcloud(); //去畸变

    if (!this->first_valid_scan) {
      return;
    }

  } else {

    this->scan_stamp = this->scan_header_stamp.toSec();

    // 在IMU数据到达之前不要处理扫描数据
    if (!this->first_valid_scan) { // tips：这个和下面的deskewPointcloud函数中判断类似

      if (this->imu_buffer.empty() ||
          this->scan_stamp <= this->imu_buffer.back().stamp) {
        return;
      }

      this->first_valid_scan = true;
      this->T_prior = this->T; // 假设第一次扫描没有运动

    } else {

      // 第二个及以后的扫描的IMU先验
      std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
          frames;
      frames = this->integrateImu(
          this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p,
          this->geo.prev_vel.cast<float>(), {this->scan_stamp}); // IMU积分

      if (frames.size() > 0) {
        this->T_prior = frames.back();
      } else {
        this->T_prior = this->T;
      }
    }

    pcl::PointCloud<PointType>::Ptr deskewed_scan_(
        boost::make_shared<pcl::PointCloud<PointType>>()); //创建一个点云指针
    pcl::transformPointCloud(
        *this->original_scan, *deskewed_scan_,
        this->T_prior *
            this->extrinsics
                .baselink2lidar_T); //将原始点云转换到baselink坐标系下
    this->deskewed_scan = deskewed_scan_;
    this->deskew_status = false;
  }

  // Voxel Grid Filter
  if (this->vf_use_) {
    pcl::PointCloud<PointType>::Ptr current_scan_(
        boost::make_shared<pcl::PointCloud<PointType>>(
            *this->deskewed_scan));           //创建一个点云指针
    this->voxel.setInputCloud(current_scan_); //设置输入点云
    this->voxel.filter(*current_scan_);       //滤波
    this->current_scan = current_scan_;
  } else {
    this->current_scan = this->deskewed_scan;
  }
}

4. deskewPointcloud—雷达逐点预积分

这个函数的作用是去除点云数据的畸变，使得点云数据的时间戳相对于一个参考时间戳。该函数的输入是一个原始的点云数据，输出是一个去畸变的点云数据。

首先，该函数创建了一个pcl::PointCloud::Ptr类型的指针deskewed_scan_，用于存储去畸变后的点云数据。然后，该函数按照时间戳对点进行排序并构建时间戳列表。具体实现是通过定义比较函数point_time_cmp和不等于函数point_time_neq，将点按照时间戳排序。如果传感器类型是OUSTER，则时间戳存储在点的t字段中；否则，时间戳存储在点的time字段中。extract_point_time函数用于从点中提取时间戳。

接下来，该函数将点按照时间戳的顺序复制到deskewed_scan_中。此处使用std::partial_sort_copy函数，将原始点云按照时间戳排序后存储在deskewed_scan_中。

随后，该函数调用了一个名为points_unique_timestamps的函数。这个函数的作用是从deskewed_scan_点云数据中提取出时间戳不相同的点，并将它们存储在一个名为points_unique_timestamps的变量中。这个函数使用了Boost库中的adaptors，首先使用indexed()将点序号和点本身组合成一个pair，然后使用adjacent_filtered()过滤掉时间戳相邻的相同点，最终得到时间戳不相同的点。

接下来，该函数从点中提取时间戳并将它们放入一个独立的列表中。unique_time_indices存储了每个时间戳的点云在deskewed_scan_中的起始和终止索引。median_pt_index为时间戳列表的中位数。

然后，该函数检查IMU数据是否已经到达，如果没有到达，则不处理扫描数据。如果传感器类型为OUSTER，则将原始点云转换到baselink坐标系下。然后，将T_prior设置为T，将原始点云转换到baselink坐标系下，并将deskewed_scan_设为原始点云。

如果IMU数据已经到达，则使用IMU先验和去斜校正。该函数调用了integrateImu函数，该函数使用IMU数据对lidar的运动进行积分，传入timestamps这个vector来获取每个点时间的IMU积分。如果积分成功，则时间戳的数量应该等于timestamps.size()。

void dlio::OdomNode::deskewPointcloud() {

  pcl::PointCloud<PointType>::Ptr deskewed_scan_(
      boost::make_shared<pcl::PointCloud<PointType>>());
  deskewed_scan_->points.resize(
      this->original_scan->points.size()); //设置点云大小

  // 各个点的时间戳应该相对于此时间
  double sweep_ref_time = this->scan_header_stamp.toSec();

  // 按时间戳对点进行排序并构建时间戳列表
  std::function<bool(const PointType &, const PointType &)>
      point_time_cmp; //比较函数
  std::function<bool(boost::range::index_value<PointType &, long>,
                     boost::range::index_value<PointType &, long>)>
      point_time_neq; //不等于函数
  std::function<double(boost::range::index_value<PointType &, long>)>
      extract_point_time; //提取时间

  if (this->sensor == dlio::SensorType::OUSTER) {
    point_time_cmp = [](const PointType &p1, const PointType &p2) {
      return p1.t < p2.t;
    }; //定义内容
    point_time_neq = [](boost::range::index_value<PointType &, long> p1,
                        boost::range::index_value<PointType &, long> p2) {
      return p1.value().t != p2.value().t;
    };
    extract_point_time =
        [&sweep_ref_time](boost::range::index_value<PointType &, long> pt) {
          return sweep_ref_time + pt.value().t * 1e-9f;
        };
  } else {
    point_time_cmp = [](const PointType &p1, const PointType &p2) {
      return p1.time < p2.time;
    };
    point_time_neq = [](boost::range::index_value<PointType &, long> p1,
                        boost::range::index_value<PointType &, long> p2) {
      return p1.value().time != p2.value().time;
    };
    extract_point_time =
        [&sweep_ref_time](boost::range::index_value<PointType &, long> pt) {
          return sweep_ref_time + pt.value().time;
        };
  }

  // 按照时间戳的顺序将点复制到deskewed_scan_中
  std::partial_sort_copy(this->original_scan->points.begin(),
                         this->original_scan->points.end(),
                         deskewed_scan_->points.begin(),
                         deskewed_scan_->points.end(), point_time_cmp);

  // 这个函数的作用是从deskewed_scan_点云数据中提取出时间戳不相同的点，并将它们存储在一个名为points_unique_timestamps的变量中。
  // 这个函数使用了Boost库中的adaptors，首先使用indexed()将点序号和点本身组合成一个pair，然后使用adjacent_filtered()过滤掉时间戳相邻的相同点，最终得到时间戳不相同的点。
  auto points_unique_timestamps =
      deskewed_scan_->points | boost::adaptors::indexed() |
      boost::adaptors::adjacent_filtered(point_time_neq);

  // 从点中提取时间戳并将它们放入一个独立的列表中
  std::vector<double> timestamps;
  std::vector<int> unique_time_indices;
  for (auto it = points_unique_timestamps.begin();
       it != points_unique_timestamps.end(); it++) {
    timestamps.push_back(extract_point_time(*it));
    unique_time_indices.push_back(it->index());
  }
  unique_time_indices.push_back(
      deskewed_scan_->points.size()); //最后一个存点的个数

  int median_pt_index = timestamps.size() / 2;
  this->scan_stamp =
      timestamps[median_pt_index]; // 将this->scan_stamp设置为中位点的时间戳

  //在IMU数据到达之前不要处理扫描数据
  if (!this->first_valid_scan) {
    if (this->imu_buffer.empty() ||
        this->scan_stamp <= this->imu_buffer.back().stamp) {
      return;
    }

    this->first_valid_scan = true;
    this->T_prior = this->T; // 假设第一次扫描时没有运动
    pcl::transformPointCloud(*deskewed_scan_, *deskewed_scan_,
                             this->T_prior * this->extrinsics.baselink2lidar_T);
    this->deskewed_scan = deskewed_scan_;
    this->deskew_status = true;
    return;
  }

  //从第二次扫描开始，使用IMU先验和去斜校正
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
      frames;
  frames = this->integrateImu(
      this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p,
      this->geo.prev_vel.cast<float>(),
      timestamps); // IMU积分，传入timestamps这个vector来获取每个点时间的IMU积分
  this->deskew_size =
      frames.size(); // 如果积分成功，则时间戳的数量应该等于timestamps.size()

  // 如果扫描开始和结束之间没有帧，则可能意味着存在同步问题
  if (frames.size() != timestamps.size()) {
    ROS_FATAL("Bad time sync between LiDAR and IMU!");

    this->T_prior = this->T; //那直接将T_prior设置为T
    pcl::transformPointCloud(
        *deskewed_scan_, *deskewed_scan_,
        this->T_prior *
            this->extrinsics
                .baselink2lidar_T); //将原始点云转换到baselink坐标系下
    this->deskewed_scan = deskewed_scan_;
    this->deskew_status = false;
    return;
  }

  // 将先验更新为扫描中间时间的估计姿态（对应于this->scan_stamp）
  this->T_prior = frames[median_pt_index];

#pragma omp parallel for num_threads(this->num_threads_) //并行计算
  for (int i = 0; i < timestamps.size(); i++) {

    Eigen::Matrix4f T =
        frames[i] * this->extrinsics.baselink2lidar_T; //设置变换矩阵

    // transform point to world frame
    for (int k = unique_time_indices[i]; k < unique_time_indices[i + 1]; k++) {
      auto &pt = deskewed_scan_->points[k]; //取出点云中的点
      pt.getVector4fMap()[3] = 1.; //将点云中的点转换为齐次坐标
      pt.getVector4fMap() =
          T * pt.getVector4fMap(); //将点云中的点转换到baselink坐标系下
    }
  }

  this->deskewed_scan = deskewed_scan_;
  this->deskew_status = true;
}

4. integrateImu—-IMU积分与点云去畸变入口

这个代码主要用来对IMU数据进行积分，得到每个点的IMU积分。首先，函数接受了一些参数，包括上一帧扫描的时间戳、上一帧估算的姿态、位置和速度，以及当前帧对应点(帧)扫描的时间戳。然后，函数会检查时间戳是否为空或者时间戳大于第一个时间戳，如果是，则返回一个空向量。接着，函数会从IMU数据中提取出时间戳不相同的点，如果IMU测量不足，则返回一个空向量。

接下来，函数会通过反向整合，找到第一个IMU样本的姿态，并计算两个IMU样本之间的时间间隔。然后，函数会根据公式4克罗内克积，推算出开始时间对应的姿态。接着，函数会计算第一和第二个IMU样本之间的平均角速度，并根据公式4计算出第二个惯性测量单元样本的方向。然后，函数会计算第一个和第二个IMU样本之间的加速度，并减去重力加速度。接着，函数会计算在前两个IMU采样之间的jerk，并根据公式4计算出第一个IMU样本的速度和位置。最后，函数会调用integrateImuInternal函数，对IMU数据进行积分，并返回一个包含每个点IMU积分的向量。

/**
 * @brief
 * 这个函数主要是用来处理IMU数据的，主要是对IMU数据进行积分，得到每个点的IMU积分
 *
 * @param start_time 上一帧扫描的时间戳
 * @param q_init 上一帧估算的姿态
 * @param p_init 上一帧估算的位置
 * @param v_init 上一帧估算的速度
 * @param sorted_timestamps 当前帧对应点(帧)扫描的时间戳
 * @return std::vector<Eigen::Matrix4f,
 * Eigen::aligned_allocator<Eigen::Matrix4f>>
 */
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init,
                             Eigen::Vector3f p_init, Eigen::Vector3f v_init,
                             const std::vector<double> &sorted_timestamps) {

  const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
      empty;

  if (sorted_timestamps.empty() || start_time > sorted_timestamps.front()) {
    // 如果时间戳为空或者时间戳大于第一个时间戳，认为无效的输入，返回空向量
    return empty;
  }

  boost::circular_buffer<ImuMeas>::reverse_iterator begin_imu_it;
  boost::circular_buffer<ImuMeas>::reverse_iterator end_imu_it;
  if (this->imuMeasFromTimeRange(start_time, sorted_timestamps.back(),
                                 begin_imu_it, end_imu_it) == false) {
    //从IMU数据中提取出时间戳不相同的点，IMU测量不足，返回空向量。
    return empty;
  }

  // 反向整合以找到第一个IMU样本的姿态
  const ImuMeas &f1 = *begin_imu_it;
  const ImuMeas &f2 = *(begin_imu_it + 1);

  // 两个IMU样本之间的时间
  double dt = f2.dt;

  // 第一个IMU样本和开始时间之间的时间间隔
  double idt = start_time - f1.stamp;

  //第一和第二个IMU样本之间的角加速度
  Eigen::Vector3f alpha_dt = f2.ang_vel - f1.ang_vel;
  Eigen::Vector3f alpha = alpha_dt / dt;

  // 首个IMU样本和起始时间之间的平均角速度（反向）
  Eigen::Vector3f omega_i = -(f1.ang_vel + 0.5 * alpha * idt);

  // 将q_init设置为第一个IMU样本的方向
  q_init = Eigen::Quaternionf(
      q_init.w() - 0.5 *
                       (q_init.x() * omega_i[0] + q_init.y() * omega_i[1] +
                        q_init.z() * omega_i[2]) *
                       idt,
      q_init.x() + 0.5 *
                       (q_init.w() * omega_i[0] - q_init.z() * omega_i[1] +
                        q_init.y() * omega_i[2]) *
                       idt,
      q_init.y() + 0.5 *
                       (q_init.z() * omega_i[0] + q_init.w() * omega_i[1] -
                        q_init.x() * omega_i[2]) *
                       idt,
      q_init.z() +
          0.5 *
              (q_init.x() * omega_i[1] - q_init.y() * omega_i[0] +
               q_init.w() * omega_i[2]) *
              idt); //根据第一个IMU推算出开始时间，对应公式4克罗内克积，省略了后面一项
  q_init.normalize();

  // 第一和第二个IMU样本之间的平均角速度
  Eigen::Vector3f omega = f1.ang_vel + 0.5 * alpha_dt;

  // 第二个惯性测量单元样本的方向
  Eigen::Quaternionf q2(
      q_init.w() - 0.5 *
                       (q_init.x() * omega[0] + q_init.y() * omega[1] +
                        q_init.z() * omega[2]) *
                       dt,
      q_init.x() + 0.5 *
                       (q_init.w() * omega[0] - q_init.z() * omega[1] +
                        q_init.y() * omega[2]) *
                       dt,
      q_init.y() + 0.5 *
                       (q_init.z() * omega[0] + q_init.w() * omega[1] -
                        q_init.x() * omega[2]) *
                       dt,
      q_init.z() + 0.5 *
                       (q_init.x() * omega[1] - q_init.y() * omega[0] +
                        q_init.w() * omega[2]) *
                       dt);
  q2.normalize();

  // 首个IMU样本的加速度
  Eigen::Vector3f a1 =
      q_init._transformVector(f1.lin_accel); //根据线加速度求出f1时刻的加速度
  a1[2] -= this->gravity_;                   //减去重力加速度

  // 第二个IMU样本的加速度
  Eigen::Vector3f a2 = q2._transformVector(f2.lin_accel);
  a2[2] -= this->gravity_;

  // 在前两个IMU采样之间的jerk
  Eigen::Vector3f j = (a2 - a1) / dt;

  // 将 v_init 设置为第一个IMU样本的速度（从 start_time 开始向后倒退），公式4
  v_init -= a1 * idt + 0.5 * j * idt * idt;

  //将p_init设置为第一个IMU样本的位置（从start_time往回走，公式4
  p_init -=
      v_init * idt + 0.5 * a1 * idt * idt + (1 / 6.) * j * idt * idt * idt;

  return this->integrateImuInternal(q_init, p_init, v_init, sorted_timestamps,
                                    begin_imu_it, end_imu_it);
}

5. imuMeasFromTimeRange—根据时间范围获取IMU测量值

下面的代码根据给定的时间范围获取IMU测量值。它接受起始时间和结束时间，以及一个IMU缓冲区的起始和结束迭代器。该函数首先检查IMU缓冲区是否为空或第一个IMU数据的时间戳是否小于结束时间。如果是，则等待最新的IMU数据。然后，函数使用迭代器找到最后一个时间戳小于结束时间的IMU数据，以及第一个时间戳大于等于开始时间的IMU数据。最后，该函数将找到的IMU数据存储在反向迭代器中，并返回true。如果找不到IMU数据，则返回false。

/**
 * @brief 根据时间范围获取IMU测量值
 *
 * @param start_time 需要的IMU测量值的开始时间
 * @param end_time 需要的IMU测量值的结束时间
 * @param begin_imu_it 对应的imu_buffer的起始索引
 * @param end_imu_it 对应的imu_buffer的结束索引
 * @return true
 * @return false
 */
bool dlio::OdomNode::imuMeasFromTimeRange(
    double start_time, double end_time,
    boost::circular_buffer<ImuMeas>::reverse_iterator &begin_imu_it,
    boost::circular_buffer<ImuMeas>::reverse_iterator &end_imu_it) {

  //如果IMU缓冲区为空或者IMU缓冲区的第一个IMU数据的时间戳小于end_time
  if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) {
    // 等待最新的IMU数据
    std::unique_lock<decltype(this->mtx_imu)> lock(this->mtx_imu); //互斥锁
    this->cv_imu_stamp.wait(lock, [this, &end_time] {
      return this->imu_buffer.front().stamp >= end_time; //等待最新的IMU数据
    }); //等待最新的IMU数据
  }

  auto imu_it =
      this->imu_buffer.begin(); // IMU数据的迭代器，直到IMU数据要在end_time之后

  auto last_imu_it = imu_it; //设置最新的imu作为最后的imu
  imu_it++;
  while (imu_it != this->imu_buffer.end() && imu_it->stamp >= end_time) {
    last_imu_it = imu_it; //不断迭代拿到之前时间的IMU数据
    imu_it++;
  }

  while (imu_it != this->imu_buffer.end() && imu_it->stamp >= start_time) {
    imu_it++; //不断迭代拿到之前时间的IMU数据,并拿到imu_it和last_imu_it之间的IMU数据
  }

  if (imu_it == this->imu_buffer.end()) {
    // 测量不足，返回false，因为没有值在end_time之后
    return false;
  }
  imu_it++;

  // 设置反向迭代器（以正向时间迭代）
  end_imu_it = boost::circular_buffer<ImuMeas>::reverse_iterator(last_imu_it);
  begin_imu_it = boost::circular_buffer<ImuMeas>::reverse_iterator(imu_it);

  return true;
}

6. integrateImuInternal—单点去畸变

下面码实现了一个IMU积分方法，可以根据所有的点以及IMU时间完成计算，并对其进行插值，以便在任何时间点都可以获得姿态。该方法接受初始姿态、位置和速度、所有点的时间戳以及IMU测量值和时间戳等参数，并返回一个表示IMU测量值对应的位姿变换矩阵的向量。

在代码中，首先对初始姿态、位置和速度进行初始化，并计算出初始加速度。然后遍历IMU测量值和时间戳，根据公式4计算四元数，并更新加速度、角加速度和平均角速度等参数。随后，根据给定的时间戳进行插值，计算出对应的四元数和位置，并将其转换为位姿变换矩阵的形式，存储在一个向量中。最后，更新位置和速度，循环直到遍历完所有的IMU测量值和时间戳。

该方法的主要思路是根据IMU测量值和时间戳计算出对应的姿态和位置信息，并进行插值，以便在任何时间点都可以获得姿态。该方法的优点是可以提高定位的精度和鲁棒性，尤其是在存在运动模糊或者缺失数据等情况下。但是该方法也存在一些缺点，例如需要较高的计算资源和较高的计算精度，同时还需要对IMU测量误差进行校准和补偿，否则可能会导致积分误差的累积。

/**
 * @brief
 * 这里可以根据所有的点以及IMU时间完成计算，并对其进行插值，以便在任何时间点都可以获得姿态。对应公式5
 *
 * @param q_init 计算得到的初始姿态
 * @param p_init 计算得到的初始位置
 * @param v_init 计算得到的初始速度
 * @param sorted_timestamps 所有点的时间戳
 * @param begin_imu_it 第一个IMU样本的迭代器
 * @param end_imu_it  最后一个IMU样本的迭代器
 * @return std::vector<Eigen::Matrix4f,
 * Eigen::aligned_allocator<Eigen::Matrix4f>>
 */
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
dlio::OdomNode::integrateImuInternal(
    Eigen::Quaternionf q_init, Eigen::Vector3f p_init, Eigen::Vector3f v_init,
    const std::vector<double> &sorted_timestamps,
    boost::circular_buffer<ImuMeas>::reverse_iterator begin_imu_it,
    boost::circular_buffer<ImuMeas>::reverse_iterator end_imu_it) {

  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
      imu_se3;

  // 初始化
  Eigen::Quaternionf q = q_init;
  Eigen::Vector3f p = p_init;
  Eigen::Vector3f v = v_init;
  Eigen::Vector3f a = q._transformVector(begin_imu_it->lin_accel);
  a[2] -= this->gravity_;

  // 遍历IMU测量值和时间戳
  auto prev_imu_it = begin_imu_it;
  auto imu_it = prev_imu_it + 1;

  auto stamp_it =
      sorted_timestamps.begin(); // 对应的所有点的索引，并将起始值放入

  for (; imu_it != end_imu_it; imu_it++) {

    const ImuMeas &f0 = *prev_imu_it;
    const ImuMeas &f = *imu_it;

    // IMU样本之间的时间
    double dt = f.dt;

    // 角加速度
    Eigen::Vector3f alpha_dt = f.ang_vel - f0.ang_vel;
    Eigen::Vector3f alpha = alpha_dt / dt;

    // 平均角速度
    Eigen::Vector3f omega = f0.ang_vel + 0.5 * alpha_dt;

    // 根据公式4计算的四元数，因为只传入了一个IMU样本，所以这里的dt就是IMU样本之间的时间
    q = Eigen::Quaternionf(
        q.w() -
            0.5 * (q.x() * omega[0] + q.y() * omega[1] + q.z() * omega[2]) * dt,
        q.x() +
            0.5 * (q.w() * omega[0] - q.z() * omega[1] + q.y() * omega[2]) * dt,
        q.y() +
            0.5 * (q.z() * omega[0] + q.w() * omega[1] - q.x() * omega[2]) * dt,
        q.z() + 0.5 * (q.x() * omega[1] - q.y() * omega[0] + q.w() * omega[2]) *
                    dt);
    q.normalize();

    // 加速度
    Eigen::Vector3f a0 = a;
    a = q._transformVector(f.lin_accel);
    a[2] -= this->gravity_;

    // Jerk
    Eigen::Vector3f j_dt = a - a0;
    Eigen::Vector3f j = j_dt / dt;

    // 为给定的时间戳进行插值
    while (stamp_it != sorted_timestamps.end() && *stamp_it <= f.stamp) {
      // 上一个IMU采样点和给定时间戳之间的时间间隔
      double idt = *stamp_it - f0.stamp;

      // 平均角速度
      Eigen::Vector3f omega_i = f0.ang_vel + 0.5 * alpha * idt;

      // 根据公式5计算的四元数
      Eigen::Quaternionf q_i(
          q.w() - 0.5 *
                      (q.x() * omega_i[0] + q.y() * omega_i[1] +
                       q.z() * omega_i[2]) *
                      idt,
          q.x() + 0.5 *
                      (q.w() * omega_i[0] - q.z() * omega_i[1] +
                       q.y() * omega_i[2]) *
                      idt,
          q.y() + 0.5 *
                      (q.z() * omega_i[0] + q.w() * omega_i[1] -
                       q.x() * omega_i[2]) *
                      idt,
          q.z() + 0.5 *
                      (q.x() * omega_i[1] - q.y() * omega_i[0] +
                       q.w() * omega_i[2]) *
                      idt);
      q_i.normalize();

      // 根据公式5计算的位置
      Eigen::Vector3f p_i =
          p + v * idt + 0.5 * a0 * idt * idt + (1 / 6.) * j * idt * idt * idt;

      // 以matrix矩阵形式表示
      Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
      T.block(0, 0, 3, 3) = q_i.toRotationMatrix();
      T.block(0, 3, 3, 1) = p_i;

      imu_se3.push_back(T);

      stamp_it++;
    }

    // Position
    p += v * dt + 0.5 * a0 * dt * dt + (1 / 6.) * j_dt * dt * dt;

    // Velocity
    v += a0 * dt + 0.5 * j_dt * dt;

    prev_imu_it = imu_it;
  }

  return imu_se3;
}