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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
|
/**
* @file don_segmentation.cpp
* Difference of Normals Example for PCL Segmentation Tutorials.
*
* @author Yani Ioannou
* @date 2012-09-24
*/
#include <string>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/organized.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/don.h>
using namespace pcl;
using namespace std;
int
main (int argc, char *argv[])
{
///The smallest scale to use in the DoN filter.
double scale1;
///The largest scale to use in the DoN filter.
double scale2;
///The minimum DoN magnitude to threshold by
double threshold;
///segment scene into clusters with given distance tolerance using euclidean clustering
double segradius;
if (argc < 6)
{
cerr << "usage: " << argv[0] << " inputfile smallscale largescale threshold segradius" << endl;
exit (EXIT_FAILURE);
}
/// the file to read from.
string infile = argv[1];
/// small scale
istringstream (argv[2]) >> scale1;
/// large scale
istringstream (argv[3]) >> scale2;
istringstream (argv[4]) >> threshold; // threshold for DoN magnitude
istringstream (argv[5]) >> segradius; // threshold for radius segmentation
// Load cloud in blob format
pcl::PCLPointCloud2 blob;
pcl::io::loadPCDFile (infile.c_str (), blob);
pcl::PointCloud<PointXYZRGB>::Ptr cloud (new pcl::PointCloud<PointXYZRGB>);
pcl::fromPCLPointCloud2 (blob, *cloud);
// Create a search tree, use KDTreee for non-organized data.
pcl::search::Search<PointXYZRGB>::Ptr tree;
if (cloud->isOrganized ())
{
tree.reset (new pcl::search::OrganizedNeighbor<PointXYZRGB> ());
}
else
{
tree.reset (new pcl::search::KdTree<PointXYZRGB> (false));
}
// Set the input pointcloud for the search tree
tree->setInputCloud (cloud);
if (scale1 >= scale2)
{
cerr << "Error: Large scale must be > small scale!" << endl;
exit (EXIT_FAILURE);
}
// Compute normals using both small and large scales at each point
pcl::NormalEstimationOMP<PointXYZRGB, PointNormal> ne;
ne.setInputCloud (cloud);
ne.setSearchMethod (tree);
/**
* NOTE: setting viewpoint is very important, so that we can ensure
* normals are all pointed in the same direction!
*/
ne.setViewPoint (std::numeric_limits<float>::max (), std::numeric_limits<float>::max (), std::numeric_limits<float>::max ());
// calculate normals with the small scale
cout << "Calculating normals for scale..." << scale1 << endl;
pcl::PointCloud<PointNormal>::Ptr normals_small_scale (new pcl::PointCloud<PointNormal>);
ne.setRadiusSearch (scale1);
ne.compute (*normals_small_scale);
// calculate normals with the large scale
cout << "Calculating normals for scale..." << scale2 << endl;
pcl::PointCloud<PointNormal>::Ptr normals_large_scale (new pcl::PointCloud<PointNormal>);
ne.setRadiusSearch (scale2);
ne.compute (*normals_large_scale);
// Create output cloud for DoN results
PointCloud<PointNormal>::Ptr doncloud (new pcl::PointCloud<PointNormal>);
copyPointCloud<PointXYZRGB, PointNormal>(*cloud, *doncloud);
cout << "Calculating DoN... " << endl;
// Create DoN operator
pcl::DifferenceOfNormalsEstimation<PointXYZRGB, PointNormal, PointNormal> don;
don.setInputCloud (cloud);
don.setNormalScaleLarge (normals_large_scale);
don.setNormalScaleSmall (normals_small_scale);
if (!don.initCompute ())
{
std::cerr << "Error: Could not initialize DoN feature operator" << std::endl;
exit (EXIT_FAILURE);
}
// Compute DoN
don.computeFeature (*doncloud);
// Save DoN features
pcl::PCDWriter writer;
writer.write<pcl::PointNormal> ("don.pcd", *doncloud, false);
// Filter by magnitude
cout << "Filtering out DoN mag <= " << threshold << "..." << endl;
// Build the condition for filtering
pcl::ConditionOr<PointNormal>::Ptr range_cond (
new pcl::ConditionOr<PointNormal> ()
);
range_cond->addComparison (pcl::FieldComparison<PointNormal>::ConstPtr (
new pcl::FieldComparison<PointNormal> ("curvature", pcl::ComparisonOps::GT, threshold))
);
// Build the filter
pcl::ConditionalRemoval<PointNormal> condrem (range_cond);
condrem.setInputCloud (doncloud);
pcl::PointCloud<PointNormal>::Ptr doncloud_filtered (new pcl::PointCloud<PointNormal>);
// Apply filter
condrem.filter (*doncloud_filtered);
doncloud = doncloud_filtered;
// Save filtered output
std::cout << "Filtered Pointcloud: " << doncloud->points.size () << " data points." << std::endl;
writer.write<pcl::PointNormal> ("don_filtered.pcd", *doncloud, false);
// Filter by magnitude
cout << "Clustering using EuclideanClusterExtraction with tolerance <= " << segradius << "..." << endl;
pcl::search::KdTree<PointNormal>::Ptr segtree (new pcl::search::KdTree<PointNormal>);
segtree->setInputCloud (doncloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointNormal> ec;
ec.setClusterTolerance (segradius);
ec.setMinClusterSize (50);
ec.setMaxClusterSize (100000);
ec.setSearchMethod (segtree);
ec.setInputCloud (doncloud);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it, j++)
{
pcl::PointCloud<PointNormal>::Ptr cloud_cluster_don (new pcl::PointCloud<PointNormal>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
{
cloud_cluster_don->points.push_back (doncloud->points[*pit]);
}
cloud_cluster_don->width = int (cloud_cluster_don->points.size ());
cloud_cluster_don->height = 1;
cloud_cluster_don->is_dense = true;
//Save cluster
cout << "PointCloud representing the Cluster: " << cloud_cluster_don->points.size () << " data points." << std::endl;
stringstream ss;
ss << "don_cluster_" << j << ".pcd";
writer.write<pcl::PointNormal> (ss.str (), *cloud_cluster_don, false);
}
}
|
