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
|
#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/filters/passthrough.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/surface/concave_hull.h>
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>),
cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>),
cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
// Build a filter to remove spurious NaNs
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 1.1);
pass.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: "
<< cloud_filtered->points.size () << " data points." << std::endl;
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.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
std::cerr << "PointCloud after segmentation has: "
<< inliers->indices.size () << " inliers." << std::endl;
// Project the model inliers
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (inliers);
proj.setInputCloud (cloud_filtered);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
std::cerr << "PointCloud after projection has: "
<< cloud_projected->points.size () << " data points." << std::endl;
// Create a Concave Hull representation of the projected inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud (cloud_projected);
chull.setAlpha (0.1);
chull.reconstruct (*cloud_hull);
std::cerr << "Concave hull has: " << cloud_hull->points.size ()
<< " data points." << std::endl;
pcl::PCDWriter writer;
writer.write ("table_scene_mug_stereo_textured_hull.pcd", *cloud_hull, false);
return (0);
}
|
