基于三维点云机器人杆件目标识别与抓取（二）

本篇博文将继续介绍点云数据的预处理内容，本篇主要介绍点云数据的分割处理。
利用点云数据的轮廓、分布、几何等特征，对数据进行分割处理，并根据数据间的不同的特征对数据进行分类。点云数据分割是进行场景目标识别的前提。经数据滤波算法处理后的点云数据，对所要识别的对象而言仍存在大量无效的背景点，因此需要点云数据分割并对不同类别的点云数据进行聚类，才能更好地进行数据特征提取与识别。针对实际检测环境，本文将通过随机采样一致性分割算法（RANSAC）分割大面积场景背景，通过聚类分割算法将场景中满足要求的对象从场景点云数据中一一提取出来，形成新的点云数据子集。
一、平面分割
1.RANSAC平面分割算法实现原理如下：

2.该算法原理示意图

二、平聚类分割
场景点云数据经RANSAC平面分割算法处理后，得到平面内与平面外的两组点云数据，根据课题任务需求，保留实验平台外的点云数据，进而与实验无关的点云数据被剔除。为了获取不同对象的场景点云数据，需要对点云数据进行欧式聚类分割，进而得到不同部件的点云数据子集。如图2-6所示，计算每个点与其相邻内点之间的欧氏距离，将距离小于给定距离阈值的点视为同类，在新类中继续进行欧式距离的计算与迭代，直到任意两类之间的欧式距离大于给定的阈值。
算法原理：

2.算法实施示意图

二、算法实施效果及实验

经数据滤波后的场景点云数据剔除了大量噪声点与离群点，接下来需要对滤波后的点云数据进行点云数据分割，进而得到多个点云数据子集。首先利用RANSAC平面分割算法剔除场景数据中实验平台等无效数据点，如图2-7所示，①表示输入的待分割的点云数据，②表示为对输入数据的深度方向进行滤波，③表示实施RANSAC平面分割算法后的平面阈值范围内的点云数据，④表示RANSAC平面分割算法后的平面阈值范围外的点云数据。
对平面阈值范围外的点云数据实施点云数据欧式聚类分割算法，如图2-8所示，①表示实施RANSAC平面分割算法后的平面阈值范围外的点云数据，②表示根据设定的阈值得到的五组聚类点云数据子集，分别由0，1，2，3，4进行编号表示。详细的点云数据平面与聚类分割数据统计如表2-2所示。

三、数据分割相关代码

#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>
#include <pcl/visualization/cloud_viewer.h>

int color_bar[][3] =
{
	{ 255,0,0},
	{ 0,255,0 },
	{ 255,255,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::PCDWriter writer;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
	reader.read("scene_truss_outplane.pcd", *cloud);
	std::cout << "  平面分割后点云: " << cloud->points.size() << " 数据点" << 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>);
	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; //输出滤波点云个数

	// 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>());
	pcl::PCDWriter writer;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(100);        //设置迭代次数
	seg.setDistanceThreshold(0.02);  //设置阈值

	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); //保存外点
		cloud_filtered = cloud_f;  //将外点数据传送给cloud_filtered
	}
*/



	// Creating the KdTree object for the search method of the extraction
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(cloud);

	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); //设置原始点云 
	ec.extract(cluster_indices);  //从点云中提取聚类

								   // 可视化部分
	pcl::visualization::PCLVisualizer viewer("实际场景聚类分割");
	// 我们将要使用的颜色
	float bckgr_gray_level = 0.0;  // 黑色
	float txt_gray_lvl = 1.0 - bckgr_gray_level;
	int num = cluster_indices.size();

	int j = 0;
	int v1(0);

	viewer.addCoordinateSystem(1.0);
	viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	int v2(0);
	viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);

	viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud", v1);

	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->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() << " 数据点 " << std::endl;
		std::stringstream ss;
		ss << "filter_after_scene_truss1_" << 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), v2);
		j++;
	}

	viewer.setBackgroundColor(0.1, 0, 0.1, v1);
	viewer.setBackgroundColor(0.0, 0.1, 0.1, v2);
	//viewer.addCoordinateSystem(0.05);
	//等待直到可视化窗口关闭。
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return (0);
}