PCL点云库学习——点云分割案例解析

该案例主要包括（点云序列获取、滤波模块、平面检测及删除模块、聚类分割模块、运动分割模块、ICP配准模块、可视化模块）
1.点云序列读取
第三方库boost，实现同一个文件内所有pcd文件的读取

std::vector pcd_files_;                          //点云序列
std::vector pcd_paths_;   //储存文件夹下的路径序列
boost::filesystem::directory_iterator end_itr;
std::string dir_ = "F:\\test";
if (boost::filesystem::is_directory(dir_))              //用于判断传入的dir_是否为目录
	{
		for (boost::filesystem::directory_iterator itr(dir_); itr != end_itr; ++itr)    //结合for循环可以遍历文件path的所有内容
		{
			std::string ext = itr->path().extension().string();     //获得文件后缀
			if (ext.compare(".pcd") == 0)
			{
				pcd_files_.push_back(itr->path().string());
				pcd_paths_.push_back(itr->path());
			}
			else
			{
				PCL_DEBUG("[PCDVideoPlayer::selectFolderButtonPressed] : found a different file\n");
			}
		}
	}
	else
	{
		PCL_ERROR("Path is not a directory\n");
		exit(-1);
	}

读取第一个点云图：

通过三维扫描设备采集得到的原始散乱点云普遍存在不均匀采样的情况，点云下采样处理能得到分布均匀的点云。使用Voxel滤波器对场景点云下采样。

			double voxel_size = 0.01;
			pcl::VoxelGrid vg;
			pcl::PointCloud::Ptr cloud_filtered1(new pcl::PointCloud);
			vg.setInputCloud(back_cloud);
			vg.setLeafSize(voxel_size, voxel_size, voxel_size);
			vg.filter(*cloud_filtered1);
			*cloud_filtered = *cloud_filtered1;
			std::cout << "show the filtered data!" << endl;
			showCloudstmp(cloud_filtered);

利用采样一致性算法检测平面点云，并将其删除。

pcl::PointIndices::Ptr tmpinliers(new pcl::PointIndices);
			double distance = 0.06, degree = 25, max = 10000;
			pcl::PointCloud::Ptr Ncloud_ground_plane(new pcl::PointCloud());
			Eigen::VectorXf coefficients;
			
			pcl::SampleConsensusModelPerpendicularPlane::Ptr model(new pcl::SampleConsensusModelPerpendicularPlane(cloud_filtered));
			model->setAxis(Eigen::Vector3f(0.0, 1.0, 0.0));   //设置所搜索平面垂直的轴 
			model->setEpsAngle(pcl::deg2rad(degree));         //设置待检测的平面模型和上述轴的最大角度
			
			pcl::RandomSampleConsensus ransac(model);
			ransac.setMaxIterations(max);    //最大迭代次数
			ransac.setDistanceThreshold(distance);  //距离阈值
			ransac.computeModel();
			ransac.getInliers(tmpinliers->indices);
			ransac.getModelCoefficients(coefficients);
			pcl::ExtractIndices extract;
			extract.setInputCloud(cloud_filtered);
			extract.setIndices(tmpinliers);
			extract.setNegative(true);
			extract.filter(*Ncloud_ground_plane);
			*cloud_filtered = *Ncloud_ground_plane;
			std::cout << "show the data after deleting ground plane!" << endl;
			showCloudstmp(cloud_filtered);

重点描述：
类SampleConsensusModelPerpendicularPlane的函数setAxis用于设置所搜索平面垂直的轴，函数setEpsAngle用于设置待检测的平面模型和上述设置轴的最大角度。在这里设置（0,1,0）即Y轴，则寻找其垂直平面X-Y平面上的平面。

去除所有平面：

double distance = 0.02, ratio = 0.8;
			int maxitter = 10000;
			pcl::SACSegmentation seg;
			pcl::PointIndices::Ptr tmpinliers(new pcl::PointIndices);
			pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
			pcl::PointCloud::Ptr cloud_plane(new pcl::PointCloud());

			seg.setOptimizeCoefficients(true);
			seg.setModelType(pcl::SACMODEL_PLANE);
			seg.setMethodType(pcl::SAC_RANSAC);
			seg.setMaxIterations(maxitter);
			seg.setDistanceThreshold(distance);
			pcl::PointCloud::Ptr  cloud_f(new pcl::PointCloud);
			int nr_points = (int)cloud_filtered->points.size();
			for (int i = 0; i < 1; i++)
			{
				seg.setInputCloud(cloud_filtered);
				seg.segment(*tmpinliers, *coefficients);
				std::cout << "plane coefficients:" << *coefficients << std::endl;
				if (tmpinliers->indices.size() == 0)
				{
					std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
					break;
				}
				pcl::ExtractIndices extract;
				extract.setInputCloud(cloud_filtered);
				extract.setIndices(tmpinliers);
				extract.setNegative(false);
				extract.filter(*cloud_plane);
				std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
				extract.setNegative(true);
				extract.filter(*cloud_f);
				*cloud_filtered = *cloud_f;
			}
		}
		std::cout << "show the data after deleting all planes!" << endl;
		showCloudstmp(cloud_filtered);

聚类分割模块，场景中剩下的物体在空间上不连续，利用聚类分割算法对场景点云进行分割，得到聚类结果记为C=(C2,…,Cm），聚类结果中包含要分割的猪体。

double min = 100, max = 100000, nab = 0.02;
		pcl::search::Search::Ptr tree = boost::shared_ptr >(new pcl::search::KdTree);
		pcl::EuclideanClusterExtraction reg;
		reg.setMinClusterSize(min);  //设置一个聚类需要的最少点数目
		reg.setMaxClusterSize(max);  //设置一个聚类需要的最大点数目
		reg.setSearchMethod(tree);                      //设置点云的搜索机制
		reg.setClusterTolerance(nab);                   //设置近邻搜索的搜索半径2cm
		reg.setInputCloud(cloud_filtered);
		std::vector  clusters;
		reg.extract(clusters);           //从点云中提取聚类，并将点云索引保存在clusters中。

整个代码

#pragma warning(disable:4996)
#include 
VTK_MODULE_INIT(vtkRenderingOpenGL);
VTK_MODULE_INIT(vtkInteractionStyle);
#define BOOST_TYPEOF_EMULATION
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace pcl::console;
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud PointCloudWithNormals;
double tstart, tstop, ttime;
std::vector pcd_files_;
std::vector pcd_paths_;
std::string dir_;
boost::shared_ptr p;
int vp_1, vp_2, vp_3;
int cidx = -100;
void
pp_callback(const pcl::visualization::PointPickingEvent& event, void* cookie)
{
	static int k;
	std::string str;
	cidx = event.getPointIndex();
	if (cidx == -1)
		return;
	pcl::PointXYZ picked_pt;
	event.getPoint(picked_pt.x, picked_pt.y, picked_pt.z);
	PCL_INFO("Point index picked: %d  - [%f, %f, %f]\n", cidx, picked_pt.x, picked_pt.y, picked_pt.z);
	str = k;
	p->addSphere(picked_pt, 0.03, 1, 0, 0, str);
	k++;

}

void showCloudsLeft(const pcl::PointCloud::Ptr cloud_source)
{
	p->removePointCloud("vp1_source");
	p->setBackgroundColor(255, 255, 255);
	PointCloudColorHandlerCustom src_h(cloud_source, 255, 0, 0);
	p->addPointCloud(cloud_source, src_h, "vp1_source", vp_1);
	print_info("visualization of source in first viewport,click q to continue\n");
	p->spin();
}
void showCloudstmp(const pcl::PointCloud::Ptr cloud_source)
{
	p->setBackgroundColor(255, 255, 255);
	PointCloudColorHandlerCustom src_h(cloud_source, 0, 0, 255);
	p->addPointCloud(cloud_source, src_h, "tmp", vp_3);
	print_info("visualization of tempresult in viewport 3,click q to continue\n");
	p->spin();
	p->removePointCloud("tmp");
}
void showCloudstmp(const pcl::PointCloud::Ptr cloud_source, Eigen::Matrix ¢roid_back, Eigen::Matrix ¢roid_forth)
{
	p->setBackgroundColor(255, 255, 255);
	PointCloudColorHandlerCustom src_h(cloud_source, 0, 0, 255);
	p->addPointCloud(cloud_source, src_h, "tmpforthROI", vp_3);
	print_info("visualization of tempresult in viewport 3,click q to continue\n");
	pcl::PointXYZ picked_ptf, picked_ptb;
	picked_ptf.x = centroid_forth[0];
	picked_ptf.y = centroid_forth[1];
	picked_ptf.z = centroid_forth[2];
	picked_ptb.x = centroid_back[0];
	picked_ptb.y = centroid_back[1];
	picked_ptb.z = centroid_back[2];
	p->addSphere(picked_ptb, 0.03, 0, 1, 0, "tmpforthsphere");
	p->addSphere(picked_ptf, 0.03, 0, 1, 0, "tmpbacksphere");
	p->spin();
	p->removePointCloud("tmpforthROI");
	p->removeShape("tmpforthsphere");
	p->removeShape("tmpbacksphere");
}

void showCloudsRight(const pcl::PointCloud::Ptr cloud_target)
{
	p->setBackgroundColor(255, 255, 255);
	p->removePointCloud("target");

	PointCloudColorHandlerCustom src_h(cloud_target, 0, 255, 0);
	p->addPointCloud(cloud_target, src_h, "target", vp_2);
	print_info("visualization of tempresult in viewport 2,click q to continue\n");
	p->spin();
}
int
main(/*int argc, char** argv*/)
{
	bool voxel_filter = true, del_plane = true, save_data = false;
	Eigen::Matrix4f toFirstT = Eigen::Matrix4f::Identity();
	tstart = (double)clock() / CLOCKS_PER_SEC;
	print_info("begin to have pcd file list\n");

	/*if (argc < 2)
	{
		print_error("Syntax is: %s input.pcd -dir E:\cow _paper_patents\mono kinect cover part\live Pig\continue\one \n", argv[0]);
		print_info("  where options are:\n");
		print_info("                     -dir X =directory of pcd sequences");
		return -1;
	}*/
	//parse_argument(argc, argv, "-dir", dir_);
	pcd_files_.clear();
	pcd_paths_.clear();

	//点云序列读取模块
	boost::filesystem::directory_iterator end_itr;
	std::string dir_ = "F:\\test";
	if (boost::filesystem::is_directory(dir_))
	{
		for (boost::filesystem::directory_iterator itr(dir_); itr != end_itr; ++itr)
		{
			std::string ext = itr->path().extension().string();
			if (ext.compare(".pcd") == 0)
			{
				pcd_files_.push_back(itr->path().string());
				pcd_paths_.push_back(itr->path());
			}
			else
			{
				PCL_DEBUG("[PCDVideoPlayer::selectFolderButtonPressed] : found a different file\n");
			}
		}
	}
	else
	{
		PCL_ERROR("Path is not a directory\n");
		exit(-1);
	}
	print_info("Have pcd file list successfully\n");
	p.reset(new pcl::visualization::PCLVisualizer( "test", "PCD viewer"));
	p->createViewPort(0.0, 0, 0.5, 0.5, vp_1);
	p->createViewPort(0.5, 0, 1.0, 0.5, vp_2);
	p->createViewPort(0.0, 0.5, 1.0, 1.0, vp_3);
	pcl::PointCloud::Ptr back_cloud(new pcl::PointCloud), forth_cloud(new pcl::PointCloud), cloud_filtered(new pcl::PointCloud), All_raws(new pcl::PointCloud), All_Traws(new pcl::PointCloud);
	pcl::PointCloud::Ptr ROI_back(new pcl::PointCloud()), ROI_forth(new pcl::PointCloud());
	std::vector> ROI_list, ROIT_list;
	std::vector T_Lforth2back;
	Eigen::Vector4f ROI_backmass, ROI_forthmass;
	int  size_squences = pcd_files_.size();
	std::cout << "Total file of squences is" << size_squences << endl;

	for (int i = 0; i < size_squences; i++)
	{
		pcl::io::loadPCDFile(pcd_files_[i], *back_cloud);
		Eigen::Quaternionf ori(1, 0, 0, 0);
		back_cloud->sensor_orientation_ = ori;
		std::cout << "after reading file :" << i + 1 << " " << endl;
		*cloud_filtered = *back_cloud;
		std::cout << "show the raw data!" << endl;
		showCloudstmp(cloud_filtered);

		//滤波模块
		if (voxel_filter == true)
		{
			double voxel_size = 0.01;
			pcl::VoxelGrid vg;
			pcl::PointCloud::Ptr cloud_filtered1(new pcl::PointCloud);
			vg.setInputCloud(back_cloud);
			vg.setLeafSize(voxel_size, voxel_size, voxel_size);
			vg.filter(*cloud_filtered1);
			*cloud_filtered = *cloud_filtered1;
			std::cout << "show the filtered data!" << endl;
			showCloudstmp(cloud_filtered);
		}

		//平面检测及删除模块
		{
			pcl::PointIndices::Ptr tmpinliers(new pcl::PointIndices);
			double distance = 0.06, degree = 25, max = 10000;
			pcl::PointCloud::Ptr Ncloud_ground_plane(new pcl::PointCloud());
			Eigen::VectorXf coefficients;
			
			pcl::SampleConsensusModelPerpendicularPlane::Ptr model(new pcl::SampleConsensusModelPerpendicularPlane(cloud_filtered));
			model->setAxis(Eigen::Vector3f(0.0, 1.0, 0.0));   //设置所搜索平面垂直的轴 
			model->setEpsAngle(pcl::deg2rad(degree));         //设置待检测的平面模型和上述轴的最大角度
			
			pcl::RandomSampleConsensus ransac(model);
			ransac.setMaxIterations(max);    //最大迭代次数
			ransac.setDistanceThreshold(distance);  //距离阈值
			ransac.computeModel();
			ransac.getInliers(tmpinliers->indices);
			ransac.getModelCoefficients(coefficients);
			pcl::ExtractIndices extract;
			extract.setInputCloud(cloud_filtered);
			extract.setIndices(tmpinliers);
			extract.setNegative(true);
			extract.filter(*Ncloud_ground_plane);
			*cloud_filtered = *Ncloud_ground_plane;
			std::cout << "show the data after deleting ground plane!" << endl;
			showCloudstmp(cloud_filtered);
		}
		if (del_plane == true)
		{
			double distance = 0.02, ratio = 0.8;
			int maxitter = 10000;
			pcl::SACSegmentation seg;
			pcl::PointIndices::Ptr tmpinliers(new pcl::PointIndices);
			pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
			pcl::PointCloud::Ptr cloud_plane(new pcl::PointCloud());

			seg.setOptimizeCoefficients(true);
			seg.setModelType(pcl::SACMODEL_PLANE);
			seg.setMethodType(pcl::SAC_RANSAC);
			seg.setMaxIterations(maxitter);
			seg.setDistanceThreshold(distance);
			pcl::PointCloud::Ptr  cloud_f(new pcl::PointCloud);
			int nr_points = (int)cloud_filtered->points.size();
			for (int i = 0; i < 1; i++)
			{
				seg.setInputCloud(cloud_filtered);
				seg.segment(*tmpinliers, *coefficients);
				std::cout << "plane coefficients:" << *coefficients << std::endl;
				if (tmpinliers->indices.size() == 0)
				{
					std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
					break;
				}
				pcl::ExtractIndices extract;
				extract.setInputCloud(cloud_filtered);
				extract.setIndices(tmpinliers);
				extract.setNegative(false);
				extract.filter(*cloud_plane);
				std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
				extract.setNegative(true);
				extract.filter(*cloud_f);
				*cloud_filtered = *cloud_f;
			}
		}
		std::cout << "show the data after deleting all planes!" << endl;
		showCloudstmp(cloud_filtered);

		//聚类分割模块
		double min = 100, max = 100000, nab = 0.02;
		pcl::search::Search::Ptr tree = boost::shared_ptr >(new pcl::search::KdTree);
		pcl::EuclideanClusterExtraction reg;
		reg.setMinClusterSize(min);  //设置一个聚类需要的最少点数目
		reg.setMaxClusterSize(max);  //设置一个聚类需要的最大点数目
		reg.setSearchMethod(tree);                      //设置点云的搜索机制
		reg.setClusterTolerance(nab);                   //设置近邻搜索的搜索半径2cm
		reg.setInputCloud(cloud_filtered);
		std::vector  clusters;
		reg.extract(clusters);           //从点云中提取聚类，并将点云索引保存在clusters中。
		int clusters_size = clusters.size();
		std::vector mass_centors;
		mass_centors.resize(clusters_size);
		for (int i = 0; i < clusters_size; i++)
		{
			pcl::compute3DCentroid(*cloud_filtered, clusters[i], mass_centors[i]);
		}
		//运动分割模块
		pcl::PointCloud::Ptr  temp_forthT(new pcl::PointCloud);
		double minimum_d = 100000;
		int minimum_g;
		if (i == 0)
		{

			p->registerPointPickingCallback(&pp_callback, static_cast (&cloud_filtered));
			p->addPointCloud(cloud_filtered, "cloud_filtered", vp_3);
			std::cout << "Please press shift+left click chose one seed get the frist ROI group!" << endl;
			p->spin();
			bool found = false;
			for (int i = 0; i < clusters_size; i++)
			{
				if (std::find(clusters.at(i).indices.begin(), clusters.at(i).indices.end(), cidx) != clusters.at(i).indices.end())
				{
					pcl::copyPointCloud(*cloud_filtered, clusters[i], *ROI_back);
					p->removePointCloud("cloud_filtered", vp_3);
					p->addPointCloud(ROI_back, "ROI_back", vp_3);
					ROI_backmass = mass_centors[i];
					found = true;
					print_info("visualization of ROI in viewport 3,click q to continue\n");
					p->spin();
					break;
				}
			}
			p->removePointCloud("ROI_back");
			continue;
		}
		else
		{
			for (int i = 0; i < clusters_size; i++)
			{
				double temp = pcl::distances::l2(mass_centors[i], ROI_backmass);
				if (temp - minimum_d < 0)
				{
					minimum_d = temp;
					minimum_g = i;
					ROI_forthmass = mass_centors[i];
				}
			}
			pcl::copyPointCloud(*cloud_filtered, clusters[minimum_g], *ROI_forth);
			showCloudstmp(ROI_forth, ROI_backmass, mass_centors[minimum_g]);
		}
		// ICP配准模块
		pcl::IterativeClosestPoint icp;
		icp.setInputCloud(ROI_forth);
		icp.setInputTarget(ROI_back);
		icp.setRANSACOutlierRejectionThreshold(0.01);
		icp.setMaxCorrespondenceDistance(0.1);
		icp.setMaximumIterations(5000);
		icp.setTransformationEpsilon(1e-8);
		icp.setEuclideanFitnessEpsilon(0.1);
		pcl::PointCloud  Final;
		icp.align(Final);
		Eigen::Matrix4f T_forth2back = icp.getFinalTransformation();
		toFirstT *= T_forth2back;
		cout << "toFirstT is " << toFirstT << endl;
		pcl::transformPointCloud(*ROI_forth, *temp_forthT, toFirstT);
		T_Lforth2back.push_back(T_forth2back);
		ROI_list.push_back(*ROI_back);
		if (i == 1)
		{
			*All_Traws += *ROI_back;
			*All_Traws += *temp_forthT;
			*All_raws += *ROI_back;
			*All_raws += *ROI_forth;
		}
		else if (i == size_squences - 1)
		{
			ROI_list.push_back(*ROI_forth);
			*All_raws += *ROI_forth;
			*All_Traws += *temp_forthT;
		}
		else
		{

			*All_Traws += *temp_forthT;
			*All_raws += *ROI_forth;
		}
		showCloudsRight(All_Traws);
		showCloudsLeft(All_raws);
		*ROI_back = *ROI_forth;
		if (save_data == true)
		{
			std::stringstream ss;
			ss << i << ".pcd";
			pcl::io::savePCDFileASCII(ss.str(), *ROI_back);
		}
		ROI_backmass = ROI_forthmass;
	}

	tstop = (double)clock() / CLOCKS_PER_SEC;
	ttime = tstop - tstart;
	std::cout << "run time is" << tstop << "seconds" << endl;

	return 0;
}