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作为线程函数，同时传递了关键帧数据和时间戳。这样做是为了避免在主循环中窃取资源。

…详情请参照古月居