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
190
191
192
193
194
195
196
|
# -*- coding: utf-8 -*-
# Difference of Normals Based Segmentation
# http://pointclouds.org/documentation/tutorials/don_segmentation.php#don-segmentation
/**
* @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;
import pcl
# ///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 intialize 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);
# }
###
|
