0. 简介

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

1. computeMetrics、computeSpaciousness、computeSpaciousness—计算度量指标

下面的代码包含了计算稀疏度和密度的两个函数。这里面的GICP相关的值都是上一帧的结果传入的。computeSpaciousness函数通过计算点云数据中点到原点的距离，求出其中位数，并对其进行平滑处理，最终将结果存入metrics.spaciousness中。而computeDensity函数则根据是否完成GICP优化，来获取机器人当前的密度值，并同样进行平滑处理，最终将结果存入metrics.density中。这两个指标都是用于评价机器人定位精度的重要指标。

void dlio::OdomNode::computeMetrics() {
  this->computeSpaciousness(); //计算稀疏度
  this->computeDensity();      //计算密度
}

void dlio::OdomNode::computeSpaciousness() {

  // 计算点的范围
  std::vector<float> ds;

  for (int i = 0; i <= this->original_scan->points.size();
       i++) { //根据getScanFromROS拿到的原始点云数据
    float d = std::sqrt(
        pow(this->original_scan->points[i].x, 2) +
        pow(this->original_scan->points[i].y, 2)); //计算点到原点的距离
    ds.push_back(d);                               //将距离存入ds
  }

  // 求中值
  std::nth_element(
      ds.begin(), ds.begin() + ds.size() / 2,
      ds.end()); // 用于在一个序列中找到第k小的元素，其中k由第二个参数指定
  float median_curr = ds[ds.size() / 2];  //对应的中值的索引
  static float median_prev = median_curr; //存入到上一个时刻的中值
  float median_lpf =
      0.95 * median_prev + 0.05 * median_curr; //？算出来还是一个值
  median_prev = median_lpf;                    //同理

  // push
  this->metrics.spaciousness.push_back(median_lpf);
}

void dlio::OdomNode::computeDensity() {

  float density;

  if (!this->geo
           .first_opt_done) { //如果第一次优化未完成（没有完成GICP），则认为没有密度
    density = 0.;
  } else {
    density = this->gicp.source_density_; //将GICP累计的density传入
  }

  static float density_prev = density;
  float density_lpf = 0.95 * density_prev + 0.05 * density;
  density_prev = density_lpf;

  this->metrics.density.push_back(density_lpf);
}

2. buildSubmap—构建子图

构建子地图的函数。子地图是由一组关键帧组成的，这些关键帧被用于后续的GICP匹配。该函数首先计算当前姿态与关键帧集合中的姿态之间的距离，以确定哪些关键帧应该被包含在子地图中。然后，它使用计算出的距离来获取前K个最近邻关键帧姿态的索引。接下来，它获取凸包索引，即提取一些不必要的关键帧。然后，它获取凸包上每个关键帧之间的距离，并获取凸包的前k个最近邻的索引。接下来，它获取凹包索引，即提取一些不必要的关键帧。然后，它获取凹包上每个关键帧之间的距离，并获取凹包的前k个最近邻的索引。最后，它连接所有子地图的点云和法向量，重新初始化子地图云和法线，并将当前子地图云赋值给子地图的点云，将当前子地图的法向量赋值给子地图的法向量，将子地图的点云赋值给gicp_temp的目标点云，将gicp_temp的目标点云的kd树赋值给子地图的kd树，将当前帧的索引赋值给上一帧的索引。如果子地图与上一次迭代时发生了变化，则将标志位置为true。并将相应的值传给GICP

void dlio::OdomNode::buildSubmap(State vehicle_state) {

  // 清除用于子地图的关键帧索引向量
  this->submap_kf_idx_curr.clear();

  // 计算当前姿态与关键帧集合中的姿态之间的距离
  std::unique_lock<decltype(this->keyframes_mutex)> lock(
      this->keyframes_mutex); //通过decltype关键字可以获得变量的类型,并加上互斥锁
  std::vector<float> ds; //用于存储当前帧与关键帧之间的距离
  std::vector<int> keyframe_nn; //用于存储当前帧与关键帧之间的索引
  for (int i = 0; i < this->num_processed_keyframes;
       i++) { //获取当前时刻所有的关键帧
    float d =
        sqrt(pow(vehicle_state.p[0] - this->keyframes[i].first.first[0], 2) +
             pow(vehicle_state.p[1] - this->keyframes[i].first.first[1], 2) +
             pow(vehicle_state.p[2] - this->keyframes[i].first.first[2],
                 2));         //计算当前帧与关键帧之间的距离
    ds.push_back(d);          //将距离存入ds
    keyframe_nn.push_back(i); //将索引存入keyframe_nn
  }
  lock.unlock();

  // 获取前K个最近邻关键帧姿态的索引
  this->pushSubmapIndices(ds, this->submap_knn_, keyframe_nn);

  // 获取凸包索引,其实就是提取一些不必要的关键帧
  this->computeConvexHull();

  // 获取凸包上每个关键帧之间的距离
  std::vector<float> convex_ds;
  for (const auto &c : this->keyframe_convex) {
    convex_ds.push_back(ds[c]); //根据对应的索引将结果压入
  }

  // 获取凸包的前k个最近邻的索引
  this->pushSubmapIndices(convex_ds, this->submap_kcv_, this->keyframe_convex);

  // 获取凹包索引,其实就是提取一些不必要的关键帧
  this->computeConcaveHull();

  // 获取凸包上每个关键帧之间的距离
  std::vector<float> concave_ds;
  for (const auto &c : this->keyframe_concave) {
    concave_ds.push_back(ds[c]);
  }

  // 获取凹包的前k个最近邻的索引
  this->pushSubmapIndices(concave_ds, this->submap_kcc_,
                          this->keyframe_concave);

  // 连接所有子地图的点云和法向量
  std::sort(this->submap_kf_idx_curr.begin(),
            this->submap_kf_idx_curr.end()); //对当前帧的索引进行排序
  auto last = std::unique(this->submap_kf_idx_curr.begin(),
                          this->submap_kf_idx_curr.end()); //去除重复的元素
  this->submap_kf_idx_curr.erase(
      last, this->submap_kf_idx_curr.end()); //删除重复的元素

  // 对当前和之前的子地图的索引列表进行排序
  std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end());
  std::sort(this->submap_kf_idx_prev.begin(), this->submap_kf_idx_prev.end());

  // 检查子地图是否与上一次迭代时发生了变化
  if (this->submap_kf_idx_curr != this->submap_kf_idx_prev) {

    this->submap_hasChanged = true; //如果发生了变化,则将标志位置为true

    // 暂停以防止从主循环中窃取资源，如果主循环正在运行。
    this->pauseSubmapBuildIfNeeded();

    // 重新初始化子地图云和法线
    pcl::PointCloud<PointType>::Ptr submap_cloud_(
        boost::make_shared<pcl::PointCloud<PointType>>());
    std::shared_ptr<nano_gicp::CovarianceList> submap_normals_(
        std::make_shared<nano_gicp::CovarianceList>());

    for (auto k : this->submap_kf_idx_curr) { //遍历当前帧的索引

      // 创建当前子地图云
      lock.lock();
      *submap_cloud_ += *this->keyframes[k].second; //将当前帧的点云压入
      lock.unlock();

      // 获取相应子地图云点的法向量
      submap_normals_->insert(std::end(*submap_normals_),
                              std::begin(*(this->keyframe_normals[k])),
                              std::end(*(this->keyframe_normals[k])));
    }

    this->submap_cloud = submap_cloud_; //将当前帧的点云赋值给子地图的点云
    this->submap_normals =
        submap_normals_; //将当前帧的法向量赋值给子地图的法向量

    // 如果主循环正在运行，请暂停以防止窃取资源
    this->pauseSubmapBuildIfNeeded();

    this->gicp_temp.setInputTarget(
        this->submap_cloud); //将子地图的点云赋值给gicp_temp的目标点云
    this->submap_kdtree =
        this->gicp_temp
            .target_kdtree_; //将gicp_temp的目标点云的kd树赋值给子地图的kd树

    this->submap_kf_idx_prev =
        this->submap_kf_idx_curr; //  将当前帧的索引赋值给上一帧的索引
  }
}

3. buildKeyframesAndSubmap—创建子图点云和法线的创建

下面代码的主要功能是创建子图，包括子图的点云和法线的创建。首先，程序获取未处理的关键帧，然后将其转换到世界坐标系下。在转换过程中，程序使用了关键帧的变换矩阵和点云数据，通过调用pcl库中的transformPointCloud函数，将关键帧点云转换到世界坐标系下。同时，程序也需要更新关键帧的协方差（法向量），将其同样转换到世界坐标系下。

在转换完成后，程序会发布关键帧。这里使用了std::thread创建了一个线程，并将发布关键帧的函数publishKeyframe作为线程函数，同时传递了关键帧数据和时间戳。这样做是为了避免在主循环中窃取资源。

最后，程序会暂停以防止从主循环中窃取资源，并调用buildSubmap函数创建子图。该函数会从当前的车辆状态中获取位姿信息，并将位姿信息与关键帧的点云数据进行匹配，生成子图。在生成子图的过程中，程序会使用nano_gicp库中的函数进行点云配准，以提高子图的精度。

/**
 * @brief 通过这个函数完成子图的创建,其中包括了子图的点云和法线的创建
 *
 * @param vehicle_state 当前的车辆状态
 */
void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) {

  // 转换新的关键帧和相关的协方差列表
  std::unique_lock<decltype(this->keyframes_mutex)> lock(this->keyframes_mutex);

  for (int i = this->num_processed_keyframes; i < this->keyframes.size();
       i++) { //遍历未处理的关键帧,并完成局部点云向全局点云的转换
    pcl::PointCloud<PointType>::ConstPtr raw_keyframe =
        this->keyframes[i].second; //获取关键帧的点云
    std::shared_ptr<const nano_gicp::CovarianceList> raw_covariances =
        this->keyframe_normals[i]; //获取关键帧的协防差
    Eigen::Matrix4f T =
        this->keyframe_transformations[i]; //获取关键帧的变换矩阵
    lock.unlock();

    Eigen::Matrix4d Td =
        T.cast<double>(); //将float类型的变换矩阵转换为double类型的变换矩阵

    pcl::PointCloud<PointType>::Ptr transformed_keyframe(
        boost::make_shared<pcl::PointCloud<PointType>>());
    pcl::transformPointCloud(*raw_keyframe, *transformed_keyframe,
                             T); //将关键帧点云转换到世界坐标系下

    std::shared_ptr<nano_gicp::CovarianceList> transformed_covariances(
        std::make_shared<nano_gicp::CovarianceList>(
            raw_covariances->size())); //创建一个新的协方差列表
    std::transform(raw_covariances->begin(), raw_covariances->end(),
                   transformed_covariances->begin(),
                   [&Td](Eigen::Matrix4d cov) {
                     return Td * cov * Td.transpose();
                   }); //将关键帧的协方差转换到世界坐标系下

    ++this->num_processed_keyframes; //更新已经处理的关键帧的数量

    lock.lock();
    this->keyframes[i].second = transformed_keyframe; //更新关键帧的点云
    this->keyframe_normals[i] = transformed_covariances; //更新关键帧的协方差(法向量)

    this->publish_keyframe_thread =
        std::thread(&dlio::OdomNode::publishKeyframe, this, this->keyframes[i],
                    this->keyframe_timestamps[i]); //发布关键帧
    this->publish_keyframe_thread.detach();
  }

  lock.unlock();

  // 暂停以防止从主循环中窃取资源，如果主循环正在运行
  this->pauseSubmapBuildIfNeeded();

  this->buildSubmap(vehicle_state); //创建子图
}

4. pushSubmapIndices—最近子图索引

下面的函数会检查输入的距离信息是否为空，如果为空则直接返回。然后，函数会维护一个最大堆，最多包含k个元素。堆的元素是距离当前帧的距离。对于每个距离信息，如果堆已经包含k个元素，并且当前距离小于堆顶的元素，则将当前距离压入堆中，并弹出堆顶元素。如果堆的元素个数少于k个，则直接将当前距离压入堆中。

然后，函数获取第k小的元素，即堆顶的元素。接着，函数将所有距离小于或等于第k小元素的元素的对应索引压入一个数组中。

该函数的实现比较简单，但是具有一定的实用性。在SLAM等应用中，需要将当前帧和最近的关键帧进行匹配，从而确定当前帧的位姿。该函数可以帮助我们快速找到距离当前帧最近的关键帧，从而提高匹配的效率。

/**
 * @brief 压入子图索引
 *
 * @param dists 距离信息
 * @param k knn的值
 * @param frames 对应的索引
 */
void dlio::OdomNode::pushSubmapIndices(std::vector<float> dists, int k,
                                       std::vector<int> frames) {

  // 确保dist不为空
  if (!dists.size()) {
    return;
  }

  // 维护最多包含 k
  // 个元素的最大堆,由于d是当前帧和所有关键帧求得,所以一开始的关键帧应该距离更远,如果处于回环的时候,有可能就达不到K个的要求
  std::priority_queue<float> pq;

  for (auto d : dists) {
    //一直和堆顶比较,如果比堆顶小,就弹出堆顶,压入新的
    if (pq.size() >= k && pq.top() > d) {
      pq.push(d);
      pq.pop();
    } else if (pq.size() < k) {
      pq.push(d);
    }
  }

  // 获取第k小的元素，它应该在堆的顶部
  float kth_element = pq.top();

  // 获取所有小于或等于第k小元素的元素进行压入
  for (int i = 0; i < dists.size(); ++i) {
    if (dists[i] <= kth_element)
      this->submap_kf_idx_curr.push_back(frames[i]);
  }
}

5. computeConvexHull&computeConcaveHull—凹凸包检测

下面的代码包括了计算凸包和凹包的函数。在SLAM系统中，关键帧是指一些特殊的帧，它们包含有足够多的特征点，可以用来计算相机的位姿。凸包和凹包则是一些形状描述工具，用于描述这些关键帧的空间分布情况。

在计算凸包和凹包时，首先需要将所有处理过的关键帧的位置信息放到一个点云中，然后对这个点云进行凸包或凹包的计算。在计算凸包和凹包时，需要设置至少4个关键帧或5个关键帧。计算出凸包和凹包之后，需要获取凸包或凹包上关键帧的索引，这些索引可以用于后续的位姿优化和地图构建。

凸包和凹包的计算是SLAM系统中的重要步骤，它们可以用于约束雷达的位姿，同时也可以提供关键帧的空间分布信息，用于后续的地图构建和路径规划。

void dlio::OdomNode::computeConvexHull() {

  // 至少需要4个凸包关键帧
  if (this->num_processed_keyframes < 4) {
    return;
  }

  // 创建一个点云，在关键帧处放置点
  pcl::PointCloud<PointType>::Ptr cloud = pcl::PointCloud<PointType>::Ptr(
      boost::make_shared<pcl::PointCloud<PointType>>());

  std::unique_lock<decltype(this->keyframes_mutex)> lock(this->keyframes_mutex);
  for (int i = 0; i < this->num_processed_keyframes; i++) {
    PointType pt;
    pt.x = this->keyframes[i].first.first[0]; //关键帧的位置
    pt.y = this->keyframes[i].first.first[1];
    pt.z = this->keyframes[i].first.first[2];
    cloud->push_back(pt);
  }
  lock.unlock();

  // 计算关键帧的凸包
  this->convex_hull.setInputCloud(cloud);

  // 获取凸包上关键帧的索引
  pcl::PointCloud<PointType>::Ptr convex_points =
      pcl::PointCloud<PointType>::Ptr(
          boost::make_shared<pcl::PointCloud<PointType>>());
  this->convex_hull.reconstruct(
      *convex_points); // 通过传入的点集来重新构建当前对象中的凸包

  pcl::PointIndices::Ptr convex_hull_point_idx =
      pcl::PointIndices::Ptr(boost::make_shared<pcl::PointIndices>());
  this->convex_hull.getHullPointIndices(
      *convex_hull_point_idx); //获取凸包上的点的索引,这个索引是在cloud中的索引

  this->keyframe_convex.clear();
  for (int i = 0; i < convex_hull_point_idx->indices.size(); ++i) {
    this->keyframe_convex.push_back(
        convex_hull_point_idx->indices[i]); //然后压入到keyframe_convex
  }
}

void dlio::OdomNode::computeConcaveHull() {

  // 至少需要4个凹包关键帧
  if (this->num_processed_keyframes < 5) {
    return;
  }

  //  创建一个点云，在关键帧处放置点
  pcl::PointCloud<PointType>::Ptr cloud = pcl::PointCloud<PointType>::Ptr(
      boost::make_shared<pcl::PointCloud<PointType>>());

  std::unique_lock<decltype(this->keyframes_mutex)> lock(this->keyframes_mutex);
  for (int i = 0; i < this->num_processed_keyframes; i++) {
    PointType pt;
    pt.x = this->keyframes[i].first.first[0]; //关键帧的位置
    pt.y = this->keyframes[i].first.first[1];
    pt.z = this->keyframes[i].first.first[2];
    cloud->push_back(pt);
  }
  lock.unlock();

  // 计算关键帧的凹包
  this->concave_hull.setInputCloud(cloud);

  // 获取凹包上关键帧的索引
  pcl::PointCloud<PointType>::Ptr concave_points =
      pcl::PointCloud<PointType>::Ptr(
          boost::make_shared<pcl::PointCloud<PointType>>());
  this->concave_hull.reconstruct(
      *concave_points); // 通过传入的点集来重新构建当前对象中的凹包

  pcl::PointIndices::Ptr concave_hull_point_idx =
      pcl::PointIndices::Ptr(boost::make_shared<pcl::PointIndices>());
  this->concave_hull.getHullPointIndices(
      *concave_hull_point_idx); //获取凹包上的点的索引,这个索引是在cloud中的索引

  this->keyframe_concave.clear();
  for (int i = 0; i < concave_hull_point_idx->indices.size(); ++i) {
    this->keyframe_concave.push_back(
        concave_hull_point_idx->indices[i]); //然后压入到keyframe_concave
  }
}