PCL库学习（12）_基于voxel_grid的欧式聚类算法

本算法在autoware中有相应的实现，这里对代码作相应的的理解

void VoxelGridBasedEuclideanClusterNodelet::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr & input_msg)
{
  // ros2pcl
  pcl::PointCloud<pcl::PointXYZ>::Ptr raw_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr);
  // 创建体素栅格对象并滤波，在不改变点云特征的前提下，减少点云的数量
  pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_map_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, 100000.0);
  voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_);
  voxel_grid_.setInputCloud(raw_pointcloud_ptr);
  voxel_grid_.setSaveLeafLayout(true);
  voxel_grid_.filter(*voxel_map_ptr);
  // 将滤波后点的z值设置为0，这样欧式空间上的分割变成二维空间，进一步提高分割效率
  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_2d_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  for (size_t i = 0; i < voxel_map_ptr->points.size(); ++i) 
  {
    pcl::PointXYZ point;
    point.x = voxel_map_ptr->points.at(i).x;
    point.y = voxel_map_ptr->points.at(i).y;
    point.z = 0.0;
    pointcloud_2d_ptr->push_back(point);
  }
  // 创建点云的kdtree数据结构
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud(pointcloud_2d_ptr);

  // 调用PCL的欧式聚类对象
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> pcl_euclidean_cluster;
  pcl_euclidean_cluster.setClusterTolerance(tolerance_);
  pcl_euclidean_cluster.setMinClusterSize(1);
  pcl_euclidean_cluster.setMaxClusterSize(max_cluster_size_);
  pcl_euclidean_cluster.setSearchMethod(tree);
  pcl_euclidean_cluster.setInputCloud(pointcloud_2d_ptr);
  pcl_euclidean_cluster.extract(cluster_indices);
  //重点在这里----------
  //这里创建了一个map容器从栅格点索引去查找聚类索引，键：voxel_grid_index,值:cluster_index
  std::unordered_map< int,  int> map;
  int cluster_idx = 0;
  //遍历每一个聚类
  for (std::vector<pcl::PointIndices>::const_iterator cluster_itr = cluster_indices.begin();cluster_itr != cluster_indices.end(); ++cluster_itr) 
  {
    //遍历每个聚类里面的栅格点索引
    for (std::vector<int>::const_iterator point_itr = cluster_itr->indices.begin();point_itr != cluster_itr->indices.end(); ++point_itr) 
    {
      //把每一个聚类里面的点索引标记为相同的聚类索引cluster_idx。eg.point_itr迭代器指向的栅格点索引都属于cluster_idx=0
      map[*point_itr] = cluster_idx;
    }
    ++cluster_idx;
  }
  //以上就完成了一个栅格点索引的分类，那么如何在真实世界下表示？
  std::vector<pcl::PointCloud<pcl::PointXYZ>> v_cluster;//最终的聚类
  v_cluster.resize(cluster_idx);
  for (size_t i = 0; i < raw_pointcloud_ptr->points.size(); ++i) 
  {
    //把真实世界下（原始输入点云）的一个点，通过voxel_grid转换为栅格索引值index，也即是在聚类算法下的点索引。真实世界下的点与栅格索引是多对一的关系。
    const int index = voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(raw_pointcloud_ptr->points.at(i).x, raw_pointcloud_ptr->points.at(i).y,raw_pointcloud_ptr->points.at(i).z));
    if (map.find(index) != map.end())//在map容器中去查找是否存在index的索引值
      //如果存在这样的一个索引值，就把相应的真实点放到对应的聚类中去；map[index]也即聚类的索引
      v_cluster.at(map[index]).points.push_back(raw_pointcloud_ptr->points.at(i));
  }
  //后面的代码比较容易理解
  // build output msg
  {
    autoware_perception_msgs::DynamicObjectWithFeatureArray output;
    output.header = input_msg->header;
    for (std::vector<pcl::PointCloud<pcl::PointXYZ>>::const_iterator cluster_itr =v_cluster.begin();cluster_itr != v_cluster.end(); ++cluster_itr) 
    {
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
      for (pcl::PointCloud<pcl::PointXYZ>::const_iterator point_itr = cluster_itr->points.begin();point_itr != cluster_itr->points.end(); ++point_itr) 
      {
        cloud_cluster->points.push_back(*point_itr);
      }
      if (min_cluster_size_ <= cloud_cluster->points.size() &&cloud_cluster->points.size() <= max_cluster_size_)
        continue;
      cloud_cluster->width = cloud_cluster->points.size();
      cloud_cluster->height = 1;
      cloud_cluster->is_dense = true;
      sensor_msgs::PointCloud2 ros_pointcloud;
      autoware_perception_msgs::DynamicObjectWithFeature feature_object;
      pcl::toROSMsg(*cloud_cluster, ros_pointcloud);
      ros_pointcloud.header = input_msg->header;
      feature_object.feature.cluster = ros_pointcloud;
      output.feature_objects.push_back(feature_object);
    }
    cluster_pub_.publish(output);
  }
}

其实也就是多了一个三维体素的索引过渡，当然也可以先通过体素滤波，再用原始的欧式聚类，比较容易理解。