【pcl学习笔记】使用ExtractIndices滤波器把平面分割存入vector进行显示

 直接上代码：

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>  //  //PCL可视化的头文件
#include <vector>


std::vector<pcl::PointCloud<pcl::PointXYZ>, Eigen::aligned_allocator<pcl::PointXYZ> > clouds_vector;

boost::shared_ptr<pcl::visualization::PCLVisualizer> Show2ViewerWindow(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud01, std::vector<pcl::PointCloud<pcl::PointXYZ>, Eigen::aligned_allocator<pcl::PointXYZ> > cloudvector)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->initCameraParameters();
	//创建视窗的标准代码

	//第一个窗口显示内容进行设定
	int v1(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer->setBackgroundColor(0, 0, 0, v1);
	viewer->addText("viewer_01", 10, 10, "v1 text", v1);
	pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ>	rgb(cloud01, 255, 0, 0);
	//pcl::visualization::PointCloudColorHandlerRGBField < pcl::PointXYZRGB>	rgb(pointCloudPtr);
	viewer->addPointCloud<pcl::PointXYZ>(cloud01, rgb, "sample cloud1", v1);

	//第二个显示内容进行设定
	int v2(1);
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer->setBackgroundColor(30, 30, 30, v2);
	viewer->addText("viewer_02", 10, 10, "v2 text", v2);
	
	for (size_t i = 0; i < cloudvector.size(); i++)
	{
		std::string string_id("ID_Plane: ");
		std::stringstream ss;
		std::string str;
		ss << i;
		ss >> str;
		string_id.append(str);
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);

		*cloud_temp = cloudvector[i];
		int color_R = static_cast<int>(255 * rand() / RAND_MAX);
		int color_G = static_cast<int>(255 * rand() / RAND_MAX);
		int color_B = static_cast<int>(255 * rand() / RAND_MAX);
		pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ> single_color(cloud_temp, color_R, color_G, color_B);
		viewer->addPointCloud<pcl::PointXYZ>(cloud_temp, single_color, string_id, v2);
	}


	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud1");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud2");
	viewer->addCoordinateSystem(1.0);
	return viewer;
}


int main(int argc, char **argv)
{

	pcl::PCLPointCloud2::Ptr cloud_blob(new pcl::PCLPointCloud2()), cloud_filtered_blob(new pcl::PCLPointCloud2());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>),
		original_cloud(new pcl::PointCloud<pcl::PointXYZ>), Plan_choose(new pcl::PointCloud<pcl::PointXYZ>);
	// 填入点云数据
	pcl::io::loadPCDFile("table_scene_lms400.pcd", *cloud_blob);
	//创建滤波器对象;使用叶大小为1cm的下采样
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud(cloud_blob);
	sor.setLeafSize(0.01f, 0.01f, 0.01f);
	sor.filter(*cloud_filtered_blob);
	//转换为模板点云

	pcl::fromPCLPointCloud2( *cloud_filtered_blob, *cloud_filtered);
	*original_cloud = *cloud_filtered;
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	//创建分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setOptimizeCoefficients(true);

	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(1000);
	seg.setDistanceThreshold(0.01);
	//创建滤波器对象
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	int i = 0,nr_points = (int) cloud_filtered->points.size();

	//创建视窗的标准代码
	while (cloud_filtered->points.size() > 0.3 * nr_points)
	{
		// 从余下的点云中分割最大平面组成部分
		seg.setInputCloud(cloud_filtered);
		seg.segment(*inliers, *coefficients);
		if (inliers->indices.size() == 0)
		{
			std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
			break;
		}
		// 分离内层
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud_p);
		//点云相加，点云合并显示
		//*Plan_choose += *cloud_p;
		clouds_vector.push_back(*cloud_p);
		// 创建滤波器对象
		extract.setNegative(true);
		extract.filter(*cloud_f);
		cloud_filtered.swap(cloud_f);
		i++;
	}


	//显示窗口函数
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer = Show2ViewerWindow(original_cloud, clouds_vector);
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	return (0);

}