PCL点云库——区域增长分割

区域增长分割

　　区域生长算法是利用了法线、曲率和颜色等信息来判断点云是否应该聚成一类，适用于特征均匀的连通目标。通过设定的约束条件并结合分割数据的融合需求，利用场景中物体的特征将不同的目标物体从场景中分割出来。PCL中调用区域生长分割类pcl::RegionGrowing，实现区域生长分割算法，算法的目标是按照平滑一致性约束条件合并足够近的点，因此算法的输出是一组集群，且每个集群上的点位于同一个光滑表面。该算法是一种基于点的法线角度差的分割，通过比较种子点与其邻域点之间的法线夹角，将小于平滑阈值(人为设定)的作为同一平滑曲面的部分。
　　算法步骤：依据点云的曲率值对输入点云进行排序，选择曲率最小的点作为初始种子点，因为该点所在的区域即为最平滑的区域，从最平滑的区域开始生长可减少分割片段的总数，提高分割效率。初始化一个空的种子点序列和空的聚类区域，从排序后的点云中选好初始种子点添加到种子点序列中，并搜索其邻域点，比较每一个邻域点与当前种子点之间的法线夹角，若所得结果小于平滑阈值(SmoothnessThreshold)，则将当前点添加到当前区域，若该邻域点曲率小于曲率阈值(CurvatureThreshold，人为设定)，则将其加入到种子点序列中，删除当前种子点，循环执行上述步骤，直到种子序列为空。
　　以下实例实现了PCL中的区域增长分割，将分割后的点云按不同颜色显示出来，保存了分割后的各片点云与各片点云的质心。

//****区域增长分割****//

#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/visualization/cloud_viewer.h>
#include <vector>
#include <string>
#include <atlstr.h>//CString头文件
using namespace std;

int
main (int argc, char** argv)
{
	pcl::PCDReader reader;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	reader.read("Test.pcd", *cloud);

	//设置默认输入参数
	int KN_normal=50; 
	//bool Bool_Cuting=false;
	float SmoothnessThreshold = 2.5, CurvatureThreshold = 20;

	//法线估计
	pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);//创建一个指向kd树搜索对象的共享指针
	pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;//创建法线估计对象
	normal_estimator.setSearchMethod (tree);//设置搜索方法
	normal_estimator.setInputCloud(cloud);//设置法线估计对象输入点集
	normal_estimator.setKSearch (KN_normal);// 设置用于法向量估计的k近邻数目
	normal_estimator.compute (*normals);//计算并输出法向量
 
	// 区域生长算法的5个参数
	pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;//创建区域生长分割对象
	reg.setMinClusterSize (100);//设置一个聚类需要的最小点数
	reg.setMaxClusterSize (1000000);//设置一个聚类需要的最大点数
	reg.setSearchMethod (tree);//设置搜索方法
	reg.setNumberOfNeighbours (15);//设置搜索的临近点数目
	reg.setInputCloud(cloud);//设置输入点云
	//pcl::IndicesPtr indices(new std::vector <int>);//创建一组索引
	//if(Bool_Cuting)reg.setIndices (indices);//通过输入参数设置，确定是否输入点云索引
	reg.setInputNormals (normals);//设置输入点云的法向量
	reg.setSmoothnessThreshold (SmoothnessThreshold / 180.0 * M_PI);//设置平滑阈值
	reg.setCurvatureThreshold (CurvatureThreshold);//设置曲率阈值
	std::vector <pcl::PointIndices> clusters;
	reg.extract (clusters);//获取聚类的结果，分割结果保存在点云索引的向量中。

	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> RegionGrow; //用于储存区域增长分割后的点云
	for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.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->points[*pit]);
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;
		RegionGrow.push_back(cloud_cluster);
	}

	//可视化
	pcl::visualization::PCLVisualizer viewer("PCLVisualizer");
	viewer.initCameraParameters();

	int v1(0);
	viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer.setBackgroundColor(128.0 / 255.0, 138.0 / 255.0, 135.0 / 255.0, v1);
	viewer.addText("Cloud before segmenting", 10, 10, "v1 test", v1);
	viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud", v1);

	int v2(0);
	viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer.setBackgroundColor(128.0 / 255.0, 138.0 / 255.0, 135.0 / 255.0, v2);
	viewer.addText("Cloud after segmenting", 10, 10, "v2 test", v2);

	ofstream fout; //c++输出文件流，支持输出c++的数据类型，如string
	fout.open("RGCentroid.txt");
	for (int i = 0; i < RegionGrow.size(); i++)
	{
		//显示分割得到的各片点云
		CString cstr;
		cstr.Format(_T("cloud_segmented%d"), i);
		cstr += _T(".pcd");
		string str_filename = CStringA(cstr);
        /*std::string str_filename = "cloud_segmented";
        str_filename += std::to_string(i);
        str_filename += ".pcd";*/	
		//显示分割得到的各片点云 
		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color(RegionGrow[i], 255 * (1 - i)*(2 - i) / 2, 255 * i*(2 - i), 255 * i*(i - 1) / 2);
		viewer.addPointCloud(RegionGrow[i], color, str_filename, v2);
		//保存分割得到的各片点云
		pcl::io::savePCDFile(str_filename, *RegionGrow[i]);
		//将分割得到的各片点云的质心保存在.txt文件中
		Eigen::Vector4f RGCentroid;
		pcl::compute3DCentroid(*RegionGrow[i], RGCentroid);//计算点云质心
		fout << "Centroid of " << str_filename << ": " << RGCentroid[0] << " " << RGCentroid[1] << " " << RGCentroid[2] << " " << endl;
	}
	fout.close(); //关闭文件。

	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	return (0);
}

　　保存下来的各片点云与点云质心文件如下所示：