pcl欧式聚类原理，源码解析pcl::EuclideanClusterExtraction

欧式聚类的代码如下：

// 创建用于提取搜索方法的kdtree树对象
pcl::search::Search<PointT>::Ptr tree = boost::shared_ptr<pcl::search::Search<PointT> > (new pcl::search::KdTree<PointT>);//创建一个指向kd树搜索对象的共享指针

//被分割出来的点云团（标号队列）
std::vector<pcl::PointIndices> clusters;
// 欧式分割器
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance(ec_ClusterTolerance); // 判定半径
ec.setMinClusterSize(ec_MinClusterSize);      // 聚类的最小点数
ec.setMaxClusterSize(ec_MaxClusterSize);      // 聚类的最大点数
//搜索策略树
ec.setSearchMethod(tree);
ec.setInputCloud(incloud);
ec.extract(clusters);
std::cout << "欧式分割结束。   " << "聚类数量：" << clusters.size() << std::endl;

欧几里德算法具体的实现方法大致是：

1 找到空间中某点p10，有kdTree找到离他最近的n个点，判断这n个点到p的距离。将距离小于阈值r的点p12,p13,p14…放入Q中；或者找距离他指定范围内的所有点，放入Q中。

2 在 Q(p10) 里找到一点p12,重复1

3 在 Q(p10,p12) 找到一点，重复1，找到p22,p23,p24…全部放进Q里

4 当 Q 再也不能有新点加入了，则完成搜索了；

关于欧几里德聚类的pcl实现函数的源码，有以下两种重载，其实大体相同。

template <typename PointT> void
pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud, 
                               const boost::shared_ptr<search::Search<PointT> > &tree,
                               float tolerance, std::vector<PointIndices> &clusters,
                               unsigned int min_pts_per_cluster, 
                               unsigned int max_pts_per_cluster)

template <typename PointT> void
pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud, 
                               const std::vector<int> &indices,
                               const boost::shared_ptr<search::Search<PointT> > &tree,
                               float tolerance, std::vector<PointIndices> &clusters,
                               unsigned int min_pts_per_cluster, 
                               unsigned int max_pts_per_cluster)

以第一种重载为例说明：

template <typename PointT> void
pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud, 
                               const boost::shared_ptr<search::Search<PointT> > &tree,
                               float tolerance, std::vector<PointIndices> &clusters,
                               unsigned int min_pts_per_cluster, 
                               unsigned int max_pts_per_cluster)
{
  if (tree->getInputCloud ()->points.size () != cloud.points.size ())    //判断kdtree是否加载失败
  {
    PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
    return;
  }
  // 检查树是否已排序——如果是，我们不需要检查第一个元素
  int nn_start_idx = tree->getSortedResults () ? 1 : 0;
  // 创建一个标记为已处理点的索引的 bool 向量，并将其初始化为 false，true为已经处理
  std::vector<bool> processed (cloud.points.size (), false);

  std::vector<int> nn_indices;              // 存储kdtree树中临近点的索引
  std::vector<float> nn_distances;          // 存储kdtree树中临近点的距离
  // Process all points in the indices vector
  for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
  {
    if (processed[i])     // 如果该点已经处理，则处理下一个点
      continue;

    std::vector<int> seed_queue;       // 已经处理的种子队列
    int sq_idx = 0;                   // 用于kdtree搜索的种子索引，每次从0开始
    seed_queue.push_back (i);           // 压入当前遍历的点

    processed[i] = true;              // 标记当前点为已处理
    
    // 遍历当前聚类中的每一个种子。 此处要注意，因为种子队列一直在变，所以用总数量做终止条件
    while (sq_idx < static_cast<int> (seed_queue.size ()))
    {
      // 寻找当前种子sq_idx的所有半径为tolerance内的点，放入seed_queue，并判断是否有点
      if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
      {
        sq_idx++;
        continue;       // 没有临近点就下一个种子
      }

      // 有临近点了，遍历找到的所有临近点
      for (size_t j = nn_start_idx; j < nn_indices.size (); ++j)             // can't assume sorted (default isn't!)
      {
        if (nn_indices[j] == -1 || processed[nn_indices[j]])        // 如果该临近点之前被处理过，则跳过
          continue;

        // 如果未处理过，压入种子队列，并标记为已经处理
        seed_queue.push_back (nn_indices[j]);
        processed[nn_indices[j]] = true;
      }

      sq_idx++;
    }

    // 找到一个聚类后，判断数量大小是否符合设定的
    if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
    {
      pcl::PointIndices r;
      r.indices.resize (seed_queue.size ());
      for (size_t j = 0; j < seed_queue.size (); ++j)
        r.indices[j] = seed_queue[j];

      // These two lines should not be needed: (can anyone confirm?) -FF
      std::sort (r.indices.begin (), r.indices.end ());
      r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());

      r.header = cloud.header;
      clusters.push_back (r);   // We could avoid a copy by working directly in the vector
    }
  }
}

可见，时间复杂度很大