pcl 欧式聚类分割

聚类方法，通过特征空间确定点与点之间的亲疏程度

算法流程：

1. 找到空间中某点p，有kdTree找到离他最近的n个点，判断这n个点到p的距离。将距离小于阈值r的点p1,p2,p3....放在类Q里；

2. 在 Q里找到一点p1,重复1，找到p22,p23,p24....全部放进Q里；

3. 当 Q 再也不能有新点加入了，则完成搜索了。

#include <pcl/ModelCoefficients.h>//采样一致性模型头文件
#include <pcl/point_types.h>//支持点类型头文件
#include <pcl/io/pcd_io.h>//pcd读写输入输出头文件
#include <pcl/filters/extract_indices.h>//索引提取
#include <pcl/filters/voxel_grid.h>//基于体素网格化的滤波
#include <pcl/features/normal_3d.h>//法线特征估计
#include <pcl/kdtree/kdtree.h>//kdtree
#include <pcl/sample_consensus/method_types.h>//console模块
#include <pcl/sample_consensus/model_types.h>//console模块
#include <pcl/segmentation/sac_segmentation.h>//基于采样一致性的分割的类的头文件
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>

int color_bar[][3] =
{
	{ 255,0,0},//红色
	{ 0,255,0 },//绿色
	{ 0,0,255 },//蓝色
	{ 0,255,255 },//青色
	{ 255,255,0 },//黄色
	{ 255,255,255 },//白色
	{ 255,0,255 }//紫色
};


int
main(int argc, char** argv)
{
	// Read in the cloud data
	//读取点云数据
	pcl::PCDReader reader;//读操作
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);//创建cloud和cloud_f指针
	reader.read("..\\..\\source\\table_scene_lms400.pcd", *cloud);//读取文件夹内的点云文件
	std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //输出滤波前点云文件的总点数

	// Create the filtering object: downsample the dataset using a leaf size of 1cm
	//创建滤波对象：使用下采样，叶尺寸设置为1cm
	pcl::VoxelGrid<pcl::PointXYZ> vg;//创建体素滤波对象
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);//创建cloud_filtered指针
	vg.setInputCloud(cloud);//设置输入点云
	vg.setLeafSize(0.01f, 0.01f, 0.01f);//设置leafsize为1cm
	vg.filter(*cloud_filtered);//执行滤波，并将结果保存在cloud_filterd中
	std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //输出滤波后点云文件的个数

	// Create the segmentation object for the planar model and set all the parameters
	//创建平面分割对象，并设置所需参数
	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>());//创建cloud_plane指针
	pcl::PCDWriter writer;//pcd写操作
	seg.setOptimizeCoefficients(true);//设置对模型做优化处理
	seg.setModelType(pcl::SACMODEL_PLANE);//设置分割模型类别为平面模型 平面的四个参数
	seg.setMethodType(pcl::SAC_RANSAC);//设置使用随机参数估计方法为随机样本共识
	seg.setMaxIterations(100);//设置最大迭代次数为100
	seg.setDistanceThreshold(0.02);//设置是否为模型内点的距离阈值为0.02

	int i = 0, nr_points = (int)cloud_filtered->points.size(); // nr_points表示原始点云数据
	// 当还多于30%原始点云数据时
	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);//判断保留内点还是反向选取，false是保留内点，true是反选

		// 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);//判断保留内点还是反向选取，false是保留内点，true是反选
		cloud_filtered = cloud_f;
	}

	// Creating the KdTree object for the search method of the extraction
	// 创建用于提取搜索方法的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);    //设置一个聚类需要的最少点数目为100
	ec.setMaxClusterSize(25000);  //设置一个聚类需要的最大点数目为25000
	ec.setSearchMethod(tree);     //设置点云的搜索机制
	ec.setInputCloud(cloud_filtered); //设置原始点云 
	ec.extract(cluster_indices);  //从点云中提取聚类

    // 可视化部分
	pcl::visualization::PCLVisualizer viewer("segmention");
	// 我们将要使用的颜色
	float bckgr_gray_level = 0;  // 黑色
	float txt_gray_lvl = 1.0 - bckgr_gray_level;
	int num = cluster_indices.size();

	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>);//创建cloud_cluster指针
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)//创建一个迭代器pit以访问第一个聚类的每一个点
			cloud_cluster->points.push_back(cloud_filtered->points[*pit]); //迭代器pit类似于一个指针，将第一个聚类分割中的每一个点进行强制类型转换，并放置在points中
		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 << "cloud_cluster_" << j << ".pcd";
		writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false); 

		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h(cloud,
			color_bar[j][0],
			color_bar[j][1],
			color_bar[j][2]);//赋予显示点云的颜色
		viewer.addPointCloud(cloud_cluster, cloud_in_color_h, std::to_string(j));

		j++;
	}

	//等待直到可视化窗口关闭
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return (0);
}
//PointCloud before filtering has : 460400 data points.
//PointCloud after filtering has : 41049 data points.
//PointCloud representing the planar component : 20536 data points.
//PointCloud representing the planar component : 12442 data points.
//PointCloud representing the Cluster : 4857 data points.
//PointCloud representing the Cluster : 1386 data points.
//PointCloud representing the Cluster : 321 data points.
//PointCloud representing the Cluster : 291 data points.
//PointCloud representing the Cluster : 123 data points.

CMakeLists文件如下所示：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(pcl_EuclideanCluster)
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (pcl_EuclideanCluster pcl_EuclideanCluster.cpp)
target_link_libraries (pcl_EuclideanCluster ${PCL_LIBRARIES})

执行结果如下图所示：

**************************************************************************************************************

 使用点云文件：

链接：https://pan.baidu.com/s/16DahXLK1sja57K627YOKPA 
提取码：8888

#include <iostream>//c++标准库的输入输出
#include <pcl/io/pcd_io.h>//pcd读写类输入输出头文件
#include <pcl/point_types.h>//支持点类型
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/cloud_viewer.h>//点云可视化支持头文件
#include <pcl/visualization/pcl_visualizer.h>  //PCL可视化的头文件

using namespace std;

typedef pcl::PointXYZRGB PointT;

int main()
{
	pcl::PointCloud<PointT>::Ptr cloud_in(new pcl::PointCloud<PointT>);//创建cloud_in指针

	pcl::PCDReader reader;
	pcl::PCDWriter writer;

	//加载点云
	if (reader.read("D:\\pclcode\\pcl_segmentation\\pcl_EuclideanCluster1\\source\\tree03.pcd", *cloud_in) < 0)
	{
		PCL_ERROR("点云文件不存在！\a\n");
		return -1;
	}

	//定义颜色数组，用于可视化
	float colors[] = {
	255, 0,   0,   // red 		1
	0,   255, 0,   // green		2
	0,   0,   255, // blue		3
	255, 255, 0,   // yellow	4
	0,   255, 255, // light blue5
	255, 0,   255, // magenta   6
	255, 255, 255, // white		7
	255, 128, 0,   // orange	8
	255, 153, 255, // pink		9
	51,  153, 255, //			10
	153, 102, 51,  //			11
	128, 51,  153, //			12
	153, 153, 51,  //			13
	163, 38,  51,  //			14
	204, 153, 102, //			15
	204, 224, 255, //			16
	128, 179, 255, //			17
	206, 255, 0,   //			18
	255, 204, 204, //			19
	204, 255, 153, //			20
	};

	
	//欧式聚类
	//创建kd树
	pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);//创建kd树对象
	tree->setInputCloud(cloud_in);//设置输入点云

	// 设置分割参数
	vector<pcl::PointIndices> cluster_indices;
	pcl::EuclideanClusterExtraction<PointT> ec;//创建欧式聚类分割对象
	ec.setClusterTolerance(0.37);	//设置近邻搜索的半径
	ec.setMinClusterSize(999);		//设置最小聚类点数
	ec.setMaxClusterSize(999999);	//设置最大聚类点数
	ec.setSearchMethod(tree);//设置搜索方法
	ec.setInputCloud(cloud_in);//设置输入点云
	ec.extract(cluster_indices);

	//----- 可视化1/3-----↓
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D viewer"));//创建可视化指针viewer

	//窗口1
	int v1(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1); //设置第一个视口在X轴、Y轴的最小值、最大值，取值在0-1之间
	viewer->setBackgroundColor(1, 1, 1, v1);
	viewer->addText("cloud_in", 10, 10, "v1 text", v1);
	viewer->addPointCloud<PointT>(cloud_in, "cloud_in", v1);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "cloud_in", v1);//设置左窗口点云为绿色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_in", v1);

	int v2(0);
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);
	viewer->addText("cloud_cluster", 10, 10, "v2 text", v2);
	//-----可视化1/3-----↑

	// 执行欧式聚类分割，并保存分割结果
	int j = 0;
	for (vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		//添加所有的点云到一个新的点云中
		//迭代容器中的点云的索引，并且分开保存索引的点云
		pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);

		for (vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)//创建一个迭代器pit以访问第一个聚类的每一个点
			cloud_cluster->points.push_back(cloud_in->points[*pit]);//迭代器pit类似于一个指针，将第一个聚类分割中的每一个点进行强制类型转换，并放置在points中
		//设置保存点云的属性问题
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;

		stringstream ss;
		ss << "tree_" << j + 1 << ".pcd";
		writer.write<PointT>(ss.str(), *cloud_cluster, true);
		cout << "-----" << ss.str() << "详情-----" << endl;
		cout << *cloud_cluster << endl;

		//-----可视化2/3-----↓
		viewer->addPointCloud<PointT>(cloud_cluster, ss.str(), v2);
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, ss.str(), v2);
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, colors[j * 3] / 255, colors[j * 3 + 1] / 255, colors[j * 3 + 2] / 255, ss.str(), v2);
		//-----可视化2/3-----↑

		j++;
	}

	//-----可视化3/3-----↓
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);//刷新
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	//-----可视化3/3-----↑

	return 0;
}
//---- - tree_1.pcd详情---- -
//header: seq: 0 stamp : 0 frame_id :
//
//	points[] : 292754
//	width : 292754
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]
//
//	---- - tree_2.pcd详情---- -
//	header : seq : 0 stamp : 0 frame_id :
//
//	points[] : 43663
//	width : 43663
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]
//
//	---- - tree_3.pcd详情---- -
//	header : seq : 0 stamp : 0 frame_id :
//
//	points[] : 13524
//	width : 13524
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]
//
//	---- - tree_4.pcd详情---- -
//	header : seq : 0 stamp : 0 frame_id :
//
//	points[] : 8692
//	width : 8692
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]
//
//	---- - tree_5.pcd详情---- -
//	header : seq : 0 stamp : 0 frame_id :
//
//	points[] : 5353
//	width : 5353
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]
//
//	---- - tree_6.pcd详情---- -
//	header : seq : 0 stamp : 0 frame_id :
//
//	points[] : 3547
//	width : 3547
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]
//
//	---- - tree_7.pcd详情---- -
//	header : seq : 0 stamp : 0 frame_id :
//
//	points[] : 2831
//	width : 2831
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]
//
//	---- - tree_8.pcd详情---- -
//	header : seq : 0 stamp : 0 frame_id :
//
//	points[] : 2446
//	width : 2446
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]
//
//	---- - tree_9.pcd详情---- -
//	header : seq : 0 stamp : 0 frame_id :
//
//	points[] : 1720
//	width : 1720
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]
//
//	---- - tree_10.pcd详情---- -
//	header : seq : 0 stamp : 0 frame_id :
//
//	points[] : 1645
//	width : 1645
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]
//
//	---- - tree_11.pcd详情---- -
//	header : seq : 0 stamp : 0 frame_id :
//
//	points[] : 1220
//	width : 1220
//	height : 1
//	is_dense : 1
//	sensor origin(xyz) : [0, 0, 0] / orientation(xyzw) : [0, 0, 0, 1]

CMakeLists文件如下所示：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(pcl_EuclideanCluster1)

find_package(PCL 1.7 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})

link_directories(${PCL_LIBRARY_DIRS})

add_definitions(${PCL_DEFINITIONS})

add_executable (pcl_EuclideanCluster pcl_EuclideanCluster.cpp)

target_link_libraries (pcl_EuclideanCluster ${PCL_LIBRARIES})

ec.setClusterTolerance(0.3);

ec.setClusterTolerance(0.35);

ec.setClusterTolerance(0.38)

ec.setClusterTolerance(0.45);

ec.setClusterTolerance(0.5);