[PCL]3 欧式距离分类EuclideanClusterExtraction

EuclideanClusterExtraction这个名字起的很奇怪，欧式距离聚类这个该如何理解？欧式距离只是一种距离测度的方法呀！有了一个Cluster在里面，我以为是某一种聚类算法，层次聚类？k-NN聚类？K-Means？还是模糊聚类？感觉很奇怪，看下代码吧。

找一个实例cluster_extraction.cpp的main入口函数。

找到computer函数，该方法中定义了一个pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;对象，接着就是参数赋值。

具体的算法执行在ec.extract (cluster_indices)中。因此进入EuclideanClusterExtraction查看具体的方法。

void compute (const pcl::PCLPointCloud2::ConstPtr &input, std::vector<pcl::PCLPointCloud2::Ptr> &output,
         int min, int max, double tolerance)
{
  // Convert data to PointCloud<T>
  PointCloud<pcl::PointXYZ>::Ptr xyz (new PointCloud<pcl::PointXYZ>);
  fromPCLPointCloud2 (*input, *xyz);

  // Estimate
  TicToc tt;
  tt.tic ();
  
  print_highlight (stderr, "Computing ");

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (xyz);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (tolerance);
  ec.setMinClusterSize (min);
  ec.setMaxClusterSize (max);
  ec.setSearchMethod (tree);
  ec.setInputCloud (xyz);
  ec.extract (cluster_indices);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cluster_indices.size ()); print_info (" clusters]\n");

  output.reserve (cluster_indices.size ());
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    pcl::ExtractIndices<pcl::PCLPointCloud2> extract;
    extract.setInputCloud (input);
    extract.setIndices (boost::make_shared<const pcl::PointIndices> (*it));
    pcl::PCLPointCloud2::Ptr out (new pcl::PCLPointCloud2);
    extract.filter (*out);
    output.push_back (out);
  }
}

可以在pcl_segmentation项目下的extract_clusters.h文件中查看EuclideanClusterExtraction的定义，可知这是一个模板类。

 template <typename PointT> 2 class EuclideanClusterExtraction: public PCLBase<PointT> 

实现文件在项目下的extract_clusters.hpp中，（还有一个extract_clusters.cpp文件），查看其中的extract方法：

template <typename PointT> void pcl::EuclideanClusterExtraction<PointT>::extract (std::vector<PointIndices> &clusters)
{
  if (!initCompute () || 
      (input_ != 0   && input_->points.empty ()) ||
      (indices_ != 0 && indices_->empty ()))
  {
    clusters.clear ();
    return;
  }

  // Initialize the spatial locator
  if (!tree_)
  {
    if (input_->isOrganized ())
      tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
    else
      tree_.reset (new pcl::search::KdTree<PointT> (false));
  }

  // Send the input dataset to the spatial locator
  tree_->setInputCloud (input_, indices_);
  extractEuclideanClusters (*input_, *indices_, tree_, static_cast<float> (cluster_tolerance_), clusters, min_pts_per_cluster_, max_pts_per_cluster_);

  //tree_->setInputCloud (input_);
  //extractEuclideanClusters (*input_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);

  // Sort the clusters based on their size (largest one first)
  std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);

  deinitCompute ();
}

 注意在extract_clusters.h中定义了四个

 template <typename PointT> void 
  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 = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());

template <typename PointT> void 
  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 = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());

 template <typename PointT, typename Normal> void 
  extractEuclideanClusters (
      const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals, 
      float tolerance, const boost::shared_ptr<KdTree<PointT> > &tree, 
      std::vector<PointIndices> &clusters, double eps_angle, 
      unsigned int min_pts_per_cluster = 1, 
      unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())

 template <typename PointT, typename Normal> 
  void extractEuclideanClusters (
      const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals, 
      const std::vector<int> &indices, const boost::shared_ptr<KdTree<PointT> > &tree, 
      float tolerance, std::vector<PointIndices> &clusters, double eps_angle, 
      unsigned int min_pts_per_cluster = 1, 
      unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())

前两个的方法实现在文件extract_clusters.hpp中，后两个直接在头文件中就以内联函数的形式实现了，两个大同小异。择其中第一个加点注释，发现其实是采用的区域生长算法实现的分割。理解错误了，区域生长需要种子点，这里应该叫层次聚类方法。

 template <typename PointT, typename Normal> void 
  extractEuclideanClusters (
      const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals, 
      float tolerance, const boost::shared_ptr<KdTree<PointT> > &tree, 
      std::vector<PointIndices> &clusters, double eps_angle, 
      unsigned int min_pts_per_cluster = 1, 
      unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
  {
    if (tree->getInputCloud ()->points.size () != cloud.points.size ())
    {
      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;
    }
    if (cloud.points.size () != normals.points.size ())
    {
      PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ());
      return;
    }

    // Create a bool vector of processed point indices, and initialize it to false
    std::vector<bool> processed (cloud.points.size (), false);

    std::vector<int> nn_indices;
    std::vector<float> nn_distances;
    // Process all points in the indices vector
    for (size_t i = 0; i < cloud.points.size (); ++i)//遍历点云中的每一个点
    {
          if (processed[i])//如果该点已经处理则跳过
            continue;

          std::vector<unsigned int> seed_queue;//定义一个种子队列
          int sq_idx = 0;
          seed_queue.push_back (static_cast<int> (i));//加入一个种子

          processed[i] = true;

          while (sq_idx < static_cast<int> (seed_queue.size ()))//遍历每一个种子
          {
            // Search for sq_idx
            if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))//没找到近邻点就继续
            {
              sq_idx++;
              continue;
            }

            for (size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
            {
              if (processed[nn_indices[j]])                         // Has this point been processed before ?种子点的近邻点中如果已经处理就跳出此次循环继续
                continue;

              //processed[nn_indices[j]] = true;
              // [-1;1]
              double dot_p = normals.points[i].normal[0] * normals.points[nn_indices[j]].normal[0] +
                             normals.points[i].normal[1] * normals.points[nn_indices[j]].normal[1] +
                             normals.points[i].normal[2] * normals.points[nn_indices[j]].normal[2];
              if ( fabs (acos (dot_p)) < eps_angle ) //根据内积求夹角，法向量都是单位向量，种子点和近邻点的法向量夹角小于阈值，
              {
                processed[nn_indices[j]] = true;
                seed_queue.push_back (nn_indices[j]);//将此种子点的临近点作为新的种子点。
              }
            }

            sq_idx++;
          }

          // If this queue is satisfactory, add to the clusters
          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
          }
    }
  }