1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
| #include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
int main(int argc,char** argv) {
/**************************************************************************************************
从输入的.pcd文件中载入数据后，创建一个VoxelGrid滤波器对数据进行下采样，在这里进行下采样时为了加快处理速度，越少的点意味着
分割循环中处理的就越快
**************************************************************************************************/
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("table_400.pcd", *cloud_blob);
std::cout << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points"
<< std::endl;
//进行体素降采样
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloud_blob);
sor.setLeafSize(0.01f, 0.01f, 0.01f);
sor.filter(*cloud_filtered_blob);
//将二进制的点云转换为xyz类型的点云，存放到目标对象指针指向的内存中
pcl::fromPCLPointCloud2(*cloud_filtered_blob,*cloud_filtered);
std::cout << "PointCloud after filtering: " << cloud_filtered_blob->width * cloud_filtered_blob->height
<< " data points" << std::endl;
//保存下采样的点云数据
pcl::PCDWriter writer;
writer.write("table_400_res.pcd", *cloud_filtered_blob);
//
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);//设定判断是否为模型内点的距离阈值
//设置ExtractIndices的实际参数
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::cout << "Cloud 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);
std::cout << "PointCloud representing the planar component: " << cloud_p->points.size() << "data points."
<< std::endl;
std::stringstream ss;
ss << "table_400_plane" << i << ".pcd";
writer.write(ss.str(), *cloud_p, false);
extract.setNegative(true);
extract.filter(*cloud_f);
cloud_filtered.swap(cloud_f);
i++;
}
return 0;
}
|
