PCL欧式距离聚类源码解析

1 .Find a point p10 in space, there is kdTree Find the n points closest to him, and judge the distance from these n points to p. Put points p12, p13, p14…. with distances less than the threshold r in class Q

Find a bit of p12 in Q (p10) and repeat 1.

3 Find a point in Q (p10, p12), repeat 1, find p22, p23, p24…. all put in Q

4. When Q can no longer add new points, the search is completed.

​
pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
                               const typename search::Search<PointT>::Ptr &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 ())   // 点数量检查
  {
    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;
  }
  // Check if the tree is sorted -- if it is we don't need to check the first element
  int nn_start_idx = tree->getSortedResults () ? 1 : 0;
  // 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 (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)   //遍历点云中的每一个点
  {
    if (processed[i])   //如果该点已经处理则跳过
      continue;
 
    std::vector<int> seed_queue;   //定义一个种子队列
 
    int sq_idx = 0;
    seed_queue.push_back (i);  //加入一个种子
 
 
    processed[i] = true;
 
    while (sq_idx < static_cast<int> (seed_queue.size ()))    //遍历每一个种子
    {
      // Search for sq_idx  kdtree 树的近邻搜索
      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]])        // Has this point been processed before ?
          continue;   // 种子点的近邻点中如果已经处理就跳出此次循环继续
 
        // Perform a simple Euclidean clustering
        seed_queue.push_back (nn_indices[j]);   //将此种子点的临近点作为新的种子点。入队操作
        processed[nn_indices[j]] = true;  // 该点已经处理，打标签
      }
 
      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
    }
  }
}

​

The C++ code is a function named `extractEuclideanClusters`, which is a part of the Point Cloud Library (PCL). This function is used to segment a point cloud into multiple clusters based on the Euclidean distance between points. Here’s a detailed explanation of the code in English:

1. **Function Signature**: The function takes several parameters:
— `cloud`: A constant reference to the input point cloud.
— `tree`: A pointer to a search object (KD-tree) that is used to find neighbors in the point cloud.
— `tolerance`: The search radius for finding neighboring points.
— `clusters`: A reference to a vector of point indices that will be filled with the resulting clusters.
— `min_pts_per_cluster`: The minimum number of points allowed for a cluster to be considered valid.
— `max_pts_per_cluster`: The maximum number of points allowed for a cluster.

2. **Point Cloud and Tree Size Check**: The function first checks if the search tree (`tree`) corresponds to the same point cloud (`cloud`). If not, an error is thrown, and the function returns early.

3. **Initialization**: The function initializes a boolean vector `processed` to keep track of which points have been assigned to a cluster. It also declares vectors for storing the nearest neighbor indices and distances.

4. **Processing Points**: The function iterates over each point in the point cloud. If a point has already been processed (assigned to a cluster), the function skips to the next point.

5. **Seed Queue**: For each unprocessed point, the function creates a seed queue and adds the point as a seed.

6. **Nearest Neighbor Search**: Using the search tree, the function performs a radius search around each seed point to find neighboring points within the specified tolerance.

7. **Clustering**: The function then iterates over the found neighbors. If a neighbor has not been processed, it is added to the seed queue and marked as processed.

8. **Cluster Size Check**: Once all neighbors for a seed have been processed, the function checks if the number of points in the seed queue is within the specified minimum and maximum cluster size.

9. **Adding to Clusters**: If the size check passes, the function creates a `pcl::PointIndices` object `r` to store the indices of the cluster. It sorts the indices and removes duplicates, then sets the cluster’s header to match the input cloud’s header, and adds the cluster to the `clusters` vector.

10. **Return**: The function does not explicitly return a value; instead, it modifies the `clusters` vector in place to contain the resulting point clusters.

This function is a core part of many point cloud segmentation tasks, such as extracting distinct objects from a scene or separating different parts of a complex object. The clusters are defined by points that are close to each other according to the Euclidean distance within the specified tolerance. The minimum and maximum cluster sizes provide a way to filter out clusters that are too small or too large, which can be useful for removing noise or for focusing on clusters of interest.