PCL 从一个点云中提取一个子集

参数化分割平面

#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/pcl_visualizer.h>

void visualize (pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target)
{
    
	pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer");

	// 创建两个显示窗口
	int v1, v2;
	viewer.createViewPort(0, 0.0, 0.5, 1.0, v1);
	viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	// 设置背景颜色
	viewer.setBackgroundColor(255, 255, 255, v1);
	viewer.setBackgroundColor(255, 255, 255, v2);

	// 给点云添加颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 0, 255);  // blue

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);  // red


	// 添加点云到显示窗口
	viewer.addPointCloud(source, source_color, "source cloud", v1);
	viewer.addPointCloud(target, target_color, "target cloud", v2);


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

}

int main(int argc, char** argv)
{
    
	//定义并实例化一个PointCloud指针对象，申明滤波前后的点云
	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>);

	//读取PCD文件
	pcl::PCDReader reader;
	reader.read("D:\\Data\\rabbit.pcd", *cloud_blob);		//读取点云文件中的数据到 cloud 对象，table_scene_lms400.pcd 文件与该cpp文件在同一级目录下

	std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;

	//创建体素栅格下采样: 下采样的大小为1cm
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;		//创建 VoxelGrid 体素栅格滤波对象
	sor.setInputCloud(cloud_blob);				//设置输入的原始采样点云数据
	sor.setLeafSize(0.01f, 0.01f, 0.01f);		//设置采样的体素大小为 1cm 立方体
	sor.filter(*cloud_filtered_blob);			//执行滤波处理，存储滤波后的输出点云 cloud_filtered_blob

	//转换为模板点云
	pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered);

	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;

	//保存采样后的点云
	pcl::PCDWriter writer;
	//writer.write<pcl::PointXYZ>("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());

	// Create the segmentation object
	pcl::SACSegmentation<pcl::PointXYZ> seg;		//创建分割对象
	// Optional
	seg.setOptimizeCoefficients(true);			//设置对估计的模型参数进行优化处理
	// Mandatory
	seg.setModelType(pcl::SACMODEL_PLANE);		//设置分割模型类别
	seg.setMethodType(pcl::SAC_RANSAC);			//设置用哪个随机参数估计方法
	seg.setMaxIterations(1000);					//设置最大迭代次数
	seg.setDistanceThreshold(0.01);				//设置判断是否为模型内点的距离阈值

	//设置ExtractIndices的实际参数
	pcl::ExtractIndices<pcl::PointXYZ> extract;

	int i = 0, nr_points = (int)cloud_filtered->size();		//点云总数
	// While 30% of the original cloud is still there
	while (cloud_filtered->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 the inliers
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud_p);
		std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;

		std::stringstream ss;
		ss << "table_scene_lms400_plane_" << i << ".pcd";
		//writer.write<pcl::PointXYZ>(ss.str(), *cloud_p, false);
		visualize(cloud_filtered, cloud_p);

		// Create the filtering object
		extract.setNegative(true);
		extract.filter(*cloud_f);
		cloud_filtered.swap(cloud_f);
		i++;
	}

	return (0);
}