PCL分割提取平面

最新推荐文章于 2024-06-21 22:56:16 发布

DF.W

最新推荐文章于 2024-06-21 22:56:16 发布

阅读量1.7k

点赞数 4

分类专栏： PCL

本文链接：https://blog.csdn.net/wdf666520/article/details/112621204

版权

PCL 专栏收录该内容

12 篇文章 3 订阅

订阅专栏

#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>

int main (int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr my_table_cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr my_table_voxel_process_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	reader.read<pcl::PointXYZ>("E:\\vs_code\\hello_pcl\\hello_pcl\\table_scene_lms400.pcd",*my_table_cloud);
	//由于原有的table点云数据量太大，进行降采样

	pcl::VoxelGrid<pcl::PointXYZ> my_voxel_filter;
	my_voxel_filter.setInputCloud(my_table_cloud);
	my_voxel_filter.setLeafSize(0.01f,0.01f,0.01f);
	my_voxel_filter.filter(*my_table_voxel_process_cloud);


	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr indices(new pcl::PointIndices());
	pcl::SACSegmentation<pcl::PointXYZ> sacseg;
	sacseg.setOptimizeCoefficients(true);
	sacseg.setModelType(pcl::SACMODEL_PLANE);
	sacseg.setMethodType(pcl::SAC_RANSAC);
	sacseg.setMaxIterations(100);
	sacseg.setDistanceThreshold(0.01);
	sacseg.setInputCloud(my_table_voxel_process_cloud);
	sacseg.segment(*indices,*coefficients);

	if (indices->indices.size() == 0)
	{
		std::cerr<<"Could not estimate a planar model for the given dataset."<<std::endl;
	}
	pcl::ExtractIndices<pcl::PointXYZ> my_extract_indices;
	my_extract_indices.setInputCloud(my_table_voxel_process_cloud);
	my_extract_indices.setIndices(indices);
	my_extract_indices.setNegative(false);
	my_extract_indices.filter(*cloud_p);

	//用于输出最后提取的点集
	for (size_t i=0;i<cloud_p->points.size();++i)
	{
		std::cout<<cloud_p->points[i].x<<" "<<cloud_p->points[i].y<<" "<<cloud_p->points[i].z<<" "<<std::endl;
	}
	//用于输出模型的系数
	std::cout<<"模型系数为： "<<coefficients->values[0]<<" "<<coefficients->values[1]<<" "<<coefficients->values[2]<<" "<<coefficients->values[3]<<std::endl;

	//下面部分用于显示
	pcl::visualization::PCLVisualizer my_viewer1;
	pcl::visualization::PCLVisualizer my_viewer2; 
	my_viewer1.addPointCloud(my_table_cloud);
	my_viewer2.addPointCloud(cloud_p);
	while(!my_viewer1.wasStopped())
	{
		my_viewer1.spinOnce(100);
		my_viewer1.spinOnce(100);
	}
	return (0);
}

由于原始的table点云数据量太大，首先利用voxel的方法对点云进行了一次精简

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr indices(new pcl::PointIndices());
	pcl::SACSegmentation<pcl::PointXYZ> sacseg;
	sacseg.setOptimizeCoefficients(true);
	sacseg.setModelType(pcl::SACMODEL_PLANE);
	sacseg.setMethodType(pcl::SAC_RANSAC);
	sacseg.setMaxIterations(100);
	sacseg.setDistanceThreshold(0.01);
	sacseg.setInputCloud(my_table_voxel_process_cloud);
	sacseg.segment(*indices,*coefficients);

这一部分首先定义了系数和索引、设置了pcl::SACSegmentation，将结果最后放在了indices,coefficients里

	pcl::ExtractIndices<pcl::PointXYZ> my_extract_indices;
	my_extract_indices.setInputCloud(my_table_voxel_process_cloud);
	my_extract_indices.setIndices(indices);
	my_extract_indices.setNegative(false);
	my_extract_indices.filter(*cloud_p);