读自动驾驶激光雷达物体检测技术（Lidar Obstacle Detection）（4）：Clustering（欧式聚类）

在第（3）实现了地面点与障碍物的分离，此部分要实现的是聚类，聚类是指把不同物体的点云分别组合聚集起来, 从而能让你跟踪汽车, 行人等多个目标. 其中一种对点云数据进行分组和聚类的方法称为欧氏聚类。

欧式聚类是指将距离紧密度高的点云聚合起来. 为了有效地进行最近邻搜索, 可以使用 KD-Tree 数据结构, 这种结构平均可以加快从 o (n)到 o (log (n))的查找时间.  这是因为Kd-Tree允许你更好地分割你的搜索空间.  通过将点分组到 KD-Tree 中的区域中, 您可以避免计算可能有数千个点的距离, 因为你知道它们不会被考虑在一个紧邻的区域中.

作者罗列了两种方式的欧式聚类第一种是自己重写了欧式聚类跟k-dtree，第二种是直接调用PCL库里边的欧式聚类。以下是两种方式的记录便于学习。

第一种：Manual Euclidean clustering

除此之外我们也可以直接使用KD-Tree进行欧氏聚类.
在此我们首先对KD-Tree的原理进行介绍. KD-Tree是一个数据结构, 由二进制树表示, 在不同维度之间对插入的点的数值进行交替比较, 通过分割区域来分割空间, 如在3D数据中, 需要交替在X,Y,Z不同轴上比较. 这样会使最近邻搜索可以快得多.
首先我们在试着二维空间上建立KD-Tree, 并讲述欧氏聚类的整个在二维空间上的实现过程, 最终我们将扩展到三维空间.

在KD-Tree中插入点(这是将点云输入到树中创建和构建KD-Tree的步骤)
假设我们需要在KD-Tree中插入４个点(-6.3, 8.4), (-6.2, 7), (-5.2, 7.1), (-5.7, 6.3)
首先我们得确定一个根点(-6.2, 7), 第0层为x轴, 需要插入的点为(-6.3, 8.4),  (-5.2, 7.1), 因为-6.3<-6.2,(-6.3, 8.4)划分为左子节点, 而-5.2>-6.2, (-5.2, 7.1)划分为右子节点. (-5.7, 6.3)中x轴-5.7>-6.2,所以放在(-5.2, 7.1)节点下, 接下来第1层使用y轴, 6.3<7.1, 因此放在(-5.2, 7.1)的左子节点. 同理, 如果我们想插入第五个点(7.2, 6.1), 你可以交替对比x,y轴数值, (7.2>-6.2)->(6.1<7.1)->(7.2>-5.7), 第五点应插入到(-5.7, 6.3)的右子节点C.
下图是KD-Tree的结构.
 

 

第二种：PCL Euclidean clustering

首先我们使用PCL内置的欧式聚类函数. 点云聚类的具体细节推荐查看PCL的官网文档。

欧氏聚类对象 ec 具有距离容忍度.  在这个距离之内的任何点都将被组合在一起.  它还有用于表示集群的点数的 min 和 max 参数.  如果一个集群真的很小, 它可能只是噪音, min参数会限制使用这个集群.  而max参数允许我们更好地分解非常大的集群,  如果一个集群非常大, 可能只是许多其他集群重叠, 最大容差可以帮助我们更好地解决对象检测.  欧式聚类的最后一个参数是 Kd-Tree.  Kd-Tree是使用输入点云创建和构建的, 这些输入点云是初始点云过滤分割后得到障碍物点云.

聚类结果可视化显示：

 

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace pcl;

int main()
{
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	pcl::PCDReader reader;
	reader.read("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\out_plane.pcd", *cloud);
	pcl::PointCloud::Ptr obstCloud(new pcl::PointCloud);
	pcl::PCDReader reader2;
	reader2.read("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\obstCloud.pcd", *obstCloud);

	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
	//tree->setInputCloud(cloud);
	tree->setInputCloud(obstCloud);
	std::vector> clusters;//保存分割后的所有类 每一类为一个点云( //创建点云索引向量，用于存储实际的点云信息)
	// 欧式聚类对检测到的障碍物进行分组
	float clusterTolerance = 0.5;
	int minsize = 10;
	int maxsize = 140;
	std::vector clusterIndices;// 创建索引类型对象
	pcl::EuclideanClusterExtraction ec; // 欧式聚类对象
	ec.setClusterTolerance(clusterTolerance);//设置近邻搜索半径
	ec.setMinClusterSize(minsize);//设置一个类需要的最小的点数
	ec.setMaxClusterSize(maxsize);//设置一个类需要的最大的点数
	ec.setSearchMethod(tree);//设置搜索方法
	//ec.setInputCloud(cloud); // feed point cloud
	ec.setInputCloud(obstCloud);//输入的点云不同，聚类的结果不同
	ec.extract(clusterIndices); // 得到所有类别的索引 clusterIndices  11类
	// 将得到的所有类的索引分别在点云中找到，即每一个索引形成了一个类
	////for (pcl::PointIndices getIndices : clusterIndices)
	////{
	////	pcl::PointCloud cloudCluster;
	////	//PtCdtr cloudCluster(new pcl::PointCloud);
	////	// For each point indice in each cluster
	////	for (int index : getIndices.indices)
	////	{
	////		cloudCluster.push_back(cloud->points[index]);
	////		//cloudCluster.push_back(obstCloud->points[index]);
	////	}
	////	cloudCluster.width = cloudCluster.size();
	////	cloudCluster.height = 1;
	////	cloudCluster.is_dense = true;
	////	clusters.push_back(cloudCluster);
	////}
	/*为了从点云索引向量中分割出每个聚类，必须迭代访问点云索引，每次创建一个新的点云数据集，并且将所有当前聚类的点写入到点云数据集中。*/
	//迭代访问点云索引clusterIndices，直到分割出所有聚类
	int j = 0;
	for (std::vector::const_iterator it = clusterIndices.begin(); it != clusterIndices.end(); ++it)
	{
		pcl::PointCloud::Ptr cloud_cluster(new pcl::PointCloud);
		//创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
		for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
			//cloud_cluster->points.push_back(cloud->points[*pit]); //查找的是定义的cloud里边对应的点云
		    cloud_cluster->points.push_back(obstCloud->points[*pit]); //查找的是定义的obstCloud里边对应的点云
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;

		std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl;
		std::stringstream ss;
		ss << "F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\cluster\\" << j << ".pcd";
		pcl::PCDWriter writer;//保存提取的点云文件
		writer.write(ss.str(), *cloud_cluster, false); //*
		j++;
	}
	
	int clusterId = 0;
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	//viewer->addPointCloud(obstCloud, "obstCLoud" ); 
	for (pcl::PointCloud cluster : clusters)
	{
		//viewer->addPointCloud(cloud, "obstCLoud" + std::to_string(clusterId));//cloud可视化显示的是平面的聚类
		viewer->addPointCloud(obstCloud, "obstCLoud" + std::to_string(clusterId));//obstCloud可视化显示的是车的聚类
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "obstCLoud" + std::to_string(clusterId));
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, "obstCLoud" + std::to_string(clusterId));
		++clusterId;
	}
	viewer->resetCamera();		//相机点重置
	viewer->spin();
	cout << clusters.size() << endl;
	system("pause");
	return 0;
}

给聚类结果画框：

结果展示：

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;
using namespace pcl;
struct Box
{
	float x_min;
	float y_min;
	float z_min;
	float x_max;
	float y_max;
	float z_max;
};
int main()
{
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	pcl::PCDReader reader;
	reader.read("D:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection代码分解\\out_plane.pcd", *cloud);
	pcl::PointCloud::Ptr all_cloud(new pcl::PointCloud);
	pcl::PCDReader reader2;
	reader2.read("D:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection代码分解\\cloudRegion.pcd", *all_cloud);
	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
	tree->setInputCloud(cloud);
	std::vector> clusters;//保存分割后的所有类 每一类为一个点云
	 // 欧式聚类对检测到的障碍物进行分组
	float clusterTolerance = 0.5;
	int minsize = 10;
	int maxsize = 140;
	std::vector clusterIndices;// 创建索引类型对象
	pcl::EuclideanClusterExtraction ec; // 欧式聚类对象
	ec.setClusterTolerance(clusterTolerance);//设置近邻搜索半径
	ec.setMinClusterSize(minsize);//设置一个类需要的最小的点数
	ec.setMaxClusterSize(maxsize);//设置一个类需要的最大的点数
	ec.setSearchMethod(tree);//设置搜索方法
	ec.setInputCloud(cloud); // feed point cloud
	ec.extract(clusterIndices); // 得到所有类别的索引 clusterIndices  11类
	// 将得到的所有类的索引分别在点云中找到，即每一个索引形成了一个类
	for (pcl::PointIndices getIndices : clusterIndices)
	{
		pcl::PointCloud cloudCluster;
		//PtCdtr cloudCluster(new pcl::PointCloud);
		// For each point indice in each cluster
		for (int index : getIndices.indices) 
		{
			cloudCluster.push_back(cloud->points[index]);
		}
		cloudCluster.width = cloudCluster.size();
		cloudCluster.height = 1;
		cloudCluster.is_dense = true;
		clusters.push_back(cloudCluster);
	}
	int clusterId = 0;

	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	//viewer->addPointCloud(all_cloud, "obstCLoud" ); //整个点云
	for (pcl::PointCloud cluster : clusters)
	{
		viewer->addPointCloud(cloud, "obstCLoud" + std::to_string(clusterId));
		viewer->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "obstCLoud" + std::to_string(clusterId) );
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, "obstCLoud" + std::to_string(clusterId));

 		pcl::PointXYZ minPoint, maxPoint;
		pcl::getMinMax3D(cluster, minPoint, maxPoint);//想得到它x,y,z三个轴上的最大值和最小值
		Box box;
		box.x_min = minPoint.x;
		box.y_min = minPoint.y;
		box.z_min = minPoint.z;
		box.x_max = maxPoint.x;
		box.y_max = maxPoint.y;
		box.z_max = maxPoint.z;
		std::string cube = "box" + std::to_string(clusterId);

		viewer->addCube(box.x_min, box.x_max, box.y_min, box.y_max, box.z_min, box.z_max,  1, 0, 0, cube);

		viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, cube); //只显示线框
		++clusterId;
	}
	viewer->resetCamera();		//相机点重置
	viewer->spin();
	cout << clusters.size() << endl;
	system("pause");
	return 0;

}