PCL源码剖析 – 欧式聚类

参考：
1. pcl Euclidean Cluster Extraction教程
2. 欧式聚类分析
3. pcl-api源码
4. 点云欧式聚类
5. 本文完整工程地址

可视化结果

在这里插入图片描述

一. 理论

聚类方法需要将无组织的点云模型P划分为更小的部分，以便显著减少P的总体处理时间。欧式空间的简单数据聚类方法可以利用固定宽度box的3D网格划分或者一般的八叉树数据结构实现。这种特殊的表征速度快，在体素表征的占据空间。然后，更普遍使用的手段是使用最近邻来实现聚类技术，本质类似于洪水填充（flood fill）算法。

二. 伪码

1. 为点云P 创建KD-Tree的输入表征
2. 创建空的聚类列表C 和 点云的检查队列Q
3. 对于P中的每一个点Pi，执行如下操作：
4.    - 将Pi添加到当前队列Q(并标记为已处理)；
5.    - while处理 Q 中的每一个Pi：
6.          - 对Pi进行近邻搜索，查找满足半径 < d 的点集合;
7.          - 检查上一步中每一个邻居点，如果标记是未被处理的点，则加入到队列Q中；
8.    - 直到Q中的所有点都被标记为已处理，将Q加入到聚类列表C中，将Q重新置为空
9. 当所有的Pi都被处理过之后结束，聚类结果为列表 C

三. 源码分析

3.1 关于主要函数extractEuclideanClusters的一些说明

功能说明
根据最大、最小聚类点数，欧式聚类阈值，将点云进行欧式聚类。
参数说明

- cloud: 输入点云
- tree: kd-tree
- tolerance: 欧式聚类距离阈值
- clusters: 输出参数，最终聚类结果数组
- min_pts_per_cluster： 每个聚类结果最小点数
- max_pts_per_cluster： 每个聚类结果最大点数

3.2 重点函数extractEuclideanClusters的实现

已为源码添加注释

 #include <pcl/segmentation/extract_clusters.h> // 分割模块
 #include <pcl/search/organized.h> // for OrganizedNeighbor
 
 //
 template <typename PointT> void
 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)
 { 
   // 若构建的kd-tree中点数，与输入点云数据点数不同，直接错误打印，并退出
   if (tree->getInputCloud ()->size () != cloud.size ())
   {
     PCL_ERROR("[pcl::extractEuclideanClusters] Tree built for a different point cloud "
               "dataset (%zu) than the input cloud (%zu)!\n",
               static_cast<std::size_t>(tree->getInputCloud()->size()),
               static_cast<std::size_t>(cloud.size()));
     return;
   }
   // 检查kdtree是否经过排序，若结果为true，则不需要检查第一个元素；否则需要检查第一个元素，默认为0
   int nn_start_idx = tree->getSortedResults () ? 1 : 0;
   // 建立处理队列Q，数目为点数数目，每一个值为false
   std::vector<bool> processed (cloud.size (), false);
   
   // nn_indices代表某一个点knn近邻查找，满足距离阈值的点云索引
   Indices nn_indices;
   std::vector<float> nn_distances;
   // 遍历每一个点
   for (int i = 0; i < static_cast<int> (cloud.size ()); ++i)
   { 
     // 若该点云被标记为true(处理过)，则跳过该点
     if (processed[i])
       continue;
     
     // 当前某一个聚类的种子队列Q
     Indices seed_queue;
     int sq_idx = 0; // 种子队列索引
     seed_queue.push_back (i); // 将当前点加入种子队列，并标记为true
     processed[i] = true; 
     
     // 循环处理种子队列Q中的每一个点，进行K近邻查找以及标记工作
     while (sq_idx < static_cast<int> (seed_queue.size ()))
     {
       // 对种子队列中索引的点进行半径距离查找，若半径内没有可查询的点，则索引+1, 跳过后续处理
       if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
       {
         sq_idx++;
         continue;
       }
       
       // 半径内有满足条件的点，点云索引保存在nn_indices。遍历这些点，判断是否处理过，若没有处理过，则加入到种子队列Q中
       for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)             // can't assume sorted (default isn't!)
       {
         // 若该索引为0 或者标记为被处理过之后，则跳过该点
         if (nn_indices[j] == UNAVAILABLE || processed[nn_indices[j]])        // Has this point been processed before ?
           continue;
  
         // 进行简单的欧式聚类，即将其加入到种子队列中，并标记为true
         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; // PointIndices类中indices是一个vector
       r.indices.resize (seed_queue.size ());
       // 完成种子队列中索引的赋值操作
       for (std::size_t j = 0; j < seed_queue.size (); ++j)
         r.indices[j] = seed_queue[j];
  
       // 点云索引值排序和去重，源码注释中表示下面两行可以去掉，并不需要
       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);   
     }
     else
     {
       // 打印聚类数目超限
       PCL_DEBUG("[pcl::extractEuclideanClusters] This cluster has %zu points, which is not between %u and %u points, so it is not a final cluster\n",
                 seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
     }
   }
 }

四. 应用

测试数据table_scene_lms400.pcd 下载
应用完整工程地址
分析：应用整体流程经历了下采样，使用平面模型分割桌面，过滤多个平面；对剩余点云进行欧式聚类，欧式聚类结果可视化。

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include<pcl/visualization/pcl_visualizer.h>

bool isPushSpace = false;

//键盘事件
void keyboard_event_occurred(const pcl::visualization::KeyboardEvent& event, void * nothing)
{
	if (event.getKeySym() == "space" && event.keyDown())
	{
		isPushSpace = true;
	}
}

int main(int argc, char** argv)
{
	// 从PCD文件中读取点云数据
	pcl::PCDReader reader;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
	reader.read("../../../data/table_scene_lms400.pcd", *cloud);
	std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*

	pcl::visualization::PCLVisualizer viewer("Cluster Extraction");

    // 注册键盘事件
	viewer.registerKeyboardCallback(&keyboard_event_occurred, (void*)NULL);
	int v1(1);
	int v2(2);
	viewer.createViewPort(0, 0, 0.5, 1, v1);
	viewer.createViewPort(0.5, 0, 1, 1, v2);

	// 使用下采样，分辨率 1cm
	pcl::VoxelGrid<pcl::PointXYZ> vg;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	vg.setInputCloud(cloud);
	vg.setLeafSize(0.01f, 0.01f, 0.01f);
	vg.filter(*cloud_filtered);
	std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*

	viewer.addPointCloud(cloud, "cloud1", v1);
	viewer.addPointCloud(cloud_filtered, "cloud2", v2);
	//渲染10秒再继续
	viewer.spinOnce(10000);

	// 创建平面分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PCDWriter writer;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(100);
	seg.setDistanceThreshold(0.02);

	// 把点云中所有的平面全部过滤掉，重复过滤，直到点云数量小于原来的0.3倍
	int i = 0, nr_points = (int)cloud_filtered->points.size();
	while (cloud_filtered->points.size() > 0.3 * nr_points)
	{
		// Segment the largest planar component from the remaining cloud
		seg.setInputCloud(cloud_filtered);
		seg.segment(*inliers, *coefficients);
		if (inliers->indices.size() == 0)
		{
			std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
			break;
		}
 
		// Extract the planar inliers from the input cloud
		pcl::ExtractIndices<pcl::PointXYZ> extract;
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false);
 
		// Write the planar inliers to disk
		extract.filter(*cloud_plane);
		std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
 
		// Remove the planar inliers, extract the rest
		extract.setNegative(true);
		extract.filter(*cloud_f);
 
		//更新显示点云
		viewer.updatePointCloud(cloud_filtered, "cloud1");
		viewer.updatePointCloud(cloud_f, "cloud2");
		//渲染3秒再继续
		viewer.spinOnce(3000);
 
		cloud_filtered = cloud_f;
 
	}

	viewer.removePointCloud("cloud2", v2);

	/// 创建KdTree对象作为搜索方法
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(cloud_filtered);

    /// 欧式聚类
	std::vector<pcl::PointIndices> cluster_indices;
	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
	ec.setClusterTolerance(0.02); // 2cm
	ec.setMinClusterSize(100);
	ec.setMaxClusterSize(25000);
	ec.setSearchMethod(tree);
	ec.setInputCloud(cloud_filtered);
	ec.extract(cluster_indices);

	//遍历抽取结果，将其显示并保存
	int j = 0;
	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		//创建临时保存点云族的点云
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
		//通过下标，逐个填充
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
			cloud_cluster->points.push_back(cloud_filtered->points[*pit]); //*

		//设置点云属性
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;

		std::cout << "当前聚类 "<<j<<" 包含的点云数量: " << cloud_cluster->points.size() << " data points." << std::endl;
		std::stringstream ss;
		ss << "cloud_cluster_" << j << ".pcd";
		// writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false); //*
		j++;

		//显示,随机设置不同颜色，以区分不同的聚类
		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cluster_color(cloud_cluster, rand()*100 + j * 80, rand() * 50 + j * 90, rand() * 200 + j * 100);
		viewer.addPointCloud(cloud_cluster,cluster_color, ss.str(), v2);
		viewer.spinOnce(5000);
	}
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
	return (0);
}