apollo 定位localization代码NDT算法模块解析(四)

ndt_solver 和ndt_voxel_grid_covariance

---

---

前言

一、SetInputTarget（）

对地图点云进行了滤波处理；
设置地图点云的体素滤波分辨率;
输入算法目标点云，即地图点云；

inline void SetInputTarget(const std::vector<Leaf> &cell_leaf,
                             const PointCloudTargetConstPtr &cloud) {
    //输入数据检查
    if (cell_leaf.empty()) {
      AWARN << "Input leaf is empty.";
      return;
    }
    if (cloud->points.empty()) {
      AWARN << "Input target is empty.";
      return;
    }
	//匹配目标点云赋值
    target_ = cloud;
    //ndt_voxel_grid_covariance 文件类
    target_cells_.SetVoxelGridResolution(resolution_, resolution_, resolution_);//设置分辨率
    target_cells_.SetInputCloud(cloud);//输入数据
    target_cells_.filter(cell_leaf, true);//滤波
  }

二 、SetInputSource（）

输入实时激光点云数据，并保存到ndt_solver 的 input_中

 inline void SetInputSource(const PointCloudTargetConstPtr &cloud) {
    if (cloud->points.empty()) {
      AWARN << "Input source is empty.";
      return;
    }
    input_ = cloud;
  }

三 、filter（）

对地图点云进行滤波，去掉小于6个点的网格（NDT算法原理决定的，），并将结果放入kdtree_,便于搜索；

//
  inline void filter(const std::vector<Leaf> &cell_leaf,
                     bool searchable = true) {
    voxel_centroids_ = PointCloudPtr(new PointCloud);
    SetMap(cell_leaf, voxel_centroids_);//进行滤波，去掉小于6个的点
    if (voxel_centroids_->size() > 0) {
      kdtree_.setInputCloud(voxel_centroids_);//将滤波处理后的地图点云放入kdtree中，便于NDT算法搜索
    }
  }

2.1.SetMap()

对地图点云进行滤波处理，去掉网格内小于6个的点，输出点云坐标（x,y,z）及对应中心点的地图坐标编号；
最后将结果保存到output中；
至少需要包含 6 个点是为了防止共面或共线，以计算 Σ的逆；

void VoxelGridCovariance<PointT>::SetMap(const std::vector<Leaf>& map_leaves,
                                         PointCloudPtr output) {
  voxel_centroids_leaf_indices_.clear();

  // Has the input dataset been set already
  //判断是否有地图点云数据
  if (!input_) {
    AWARN << "No input dataset given. ";
    output->width = output->height = 0;
    output->points.clear();
    return;
  }

  // Copy the header + allocate enough space for points
  output->height = 1;
  output->is_dense = true;//不包涵Inf/NaN这种值无限值(包含为false)
  output->points.clear();

  // Get the minimum and maximum dimensions
  Eigen::Vector4f min_p, max_p;
  pcl::getMinMax3D<PointT>(*input_, min_p, max_p); //获取最大、最小尺寸,即地图点最大，最小值

  Eigen::Vector4f left_top = Eigen::Vector4f::Zero();
  left_top.block<3, 1>(0, 0) = map_left_top_corner_.cast<float>();//类型转换
  min_p -= left_top;
  max_p -= left_top;

  // Compute the minimum and maximum bounding box values
  //将x,y，z 转为 box 值,类似栅格地图像素值
  min_b_[0] = static_cast<int>(min_p[0] * inverse_leaf_size_[0]); 
  max_b_[0] = static_cast<int>(max_p[0] * inverse_leaf_size_[0]);
  min_b_[1] = static_cast<int>(min_p[1] * inverse_leaf_size_[1]);
  max_b_[1] = static_cast<int>(max_p[1] * inverse_leaf_size_[1]);
  min_b_[2] = static_cast<int>(min_p[2] * inverse_leaf_size_[2]);
  max_b_[2] = static_cast<int>(max_p[2] * inverse_leaf_size_[2]);

  // Compute the number of divisions needed along all axis
  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones(); //计算沿所有轴所需的分段数
  div_b_[3] = 0;

  // Set up the division multiplier
  divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0);

  // Clear the leaves
  leaves_.clear();

  output->points.reserve(map_leaves.size());
  voxel_centroids_leaf_indices_.reserve(leaves_.size());

  for (unsigned int i = 0; i < map_leaves.size(); ++i) {
    const Leaf& cell_leaf = map_leaves[i];
    Eigen::Vector3d local_mean = cell_leaf.mean_ - map_left_top_corner_;
    int ijk0 =
        static_cast<int>(local_mean(0) * inverse_leaf_size_[0]) - min_b_[0];
    int ijk1 =
        static_cast<int>(local_mean(1) * inverse_leaf_size_[1]) - min_b_[1];
    int ijk2 =
        static_cast<int>(local_mean(2) * inverse_leaf_size_[2]) - min_b_[2];

    // Compute the centroid leaf index
    int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];//求中心点对应的index 为什么这样弄？地图的格式原因？

    Leaf& leaf = leaves_[idx];
    leaf = cell_leaf;
	//体素中包含的点必须大于6个
    if (cell_leaf.nr_points_ >= min_points_per_voxel_) {
      output->push_back(PointT());
      output->points.back().x = static_cast<float>(leaf.mean_[0]);
      output->points.back().y = static_cast<float>(leaf.mean_[1]);
      output->points.back().z = static_cast<float>(leaf.mean_[2]);
      voxel_centroids_leaf_indices_.push_back(idx);
    }
  }
  output->width = static_cast<uint32_t>(output->points.size());
}

总结

版权申明：转载请注明出处，严禁用于商业用途。