Point Cloud Library (PCL)  1.12.0
unary_classifier.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  *
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the copyright holder(s) nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author : Christian Potthast
36  * Email : potthast@usc.edu
37  *
38  */
39 
40 #ifndef PCL_UNARY_CLASSIFIER_HPP_
41 #define PCL_UNARY_CLASSIFIER_HPP_
42 
43 #include <Eigen/Core>
44 #include <flann/flann.hpp> // for flann::Index
45 #include <flann/algorithms/dist.h> // for flann::ChiSquareDistance
46 #include <flann/algorithms/linear_index.h> // for flann::LinearIndexParams
47 #include <flann/util/matrix.h> // for flann::Matrix
48 
49 #include <pcl/segmentation/unary_classifier.h>
50 #include <pcl/common/io.h>
51 
52 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
53 template <typename PointT>
55  input_cloud_ (new pcl::PointCloud<PointT>),
56  label_field_ (false),
57  normal_radius_search_ (0.01f),
58  fpfh_radius_search_ (0.05f),
59  feature_threshold_ (5.0)
60 {
61 }
62 
63 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
64 template <typename PointT>
66 {
67 }
68 
69 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT> void
72 {
73  input_cloud_ = input_cloud;
74 
76  std::vector<pcl::PCLPointField> fields;
77 
78  int label_index = -1;
79  label_index = pcl::getFieldIndex<PointT> ("label", fields);
80 
81  if (label_index != -1)
82  label_field_ = true;
83 }
84 
85 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
86 template <typename PointT> void
89 {
90  // resize points of output cloud
91  out->points.resize (in->size ());
92  out->width = out->size ();
93  out->height = 1;
94  out->is_dense = false;
95 
96  for (std::size_t i = 0; i < in->size (); i++)
97  {
98  pcl::PointXYZ point;
99  // fill X Y Z
100  point.x = (*in)[i].x;
101  point.y = (*in)[i].y;
102  point.z = (*in)[i].z;
103  (*out)[i] = point;
104  }
105 }
106 
107 template <typename PointT> void
110 {
111  // TODO:: check if input cloud has RGBA information and insert into the cloud
112 
113  // resize points of output cloud
114  out->points.resize (in->size ());
115  out->width = out->size ();
116  out->height = 1;
117  out->is_dense = false;
118 
119  for (std::size_t i = 0; i < in->size (); i++)
120  {
121  pcl::PointXYZRGBL point;
122  // X Y Z R G B L
123  point.x = (*in)[i].x;
124  point.y = (*in)[i].y;
125  point.z = (*in)[i].z;
126  //point.rgba = (*in)[i].rgba;
127  point.label = 1;
128  (*out)[i] = point;
129  }
130 }
131 
132 
133 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
134 template <typename PointT> void
136  std::vector<int> &cluster_numbers)
137 {
138  // find the 'label' field index
139  std::vector <pcl::PCLPointField> fields;
140  int label_idx = -1;
142  label_idx = pcl::getFieldIndex<PointT> ("label", fields);
143 
144  if (label_idx != -1)
145  {
146  for (const auto& point: *in)
147  {
148  // get the 'label' field
149  std::uint32_t label;
150  memcpy (&label, reinterpret_cast<const char*> (&point) + fields[label_idx].offset, sizeof(std::uint32_t));
151 
152  // check if label exist
153  bool exist = false;
154  for (const int &cluster_number : cluster_numbers)
155  {
156  if (static_cast<std::uint32_t> (cluster_number) == label)
157  {
158  exist = true;
159  break;
160  }
161  }
162  if (!exist)
163  cluster_numbers.push_back (label);
164  }
165  }
166 }
167 
168 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
169 template <typename PointT> void
172  int label_num)
173 {
174  // find the 'label' field index
175  std::vector <pcl::PCLPointField> fields;
176  int label_idx = -1;
177  label_idx = pcl::getFieldIndex<PointT> ("label", fields);
178 
179  if (label_idx != -1)
180  {
181  for (const auto& point : (*in))
182  {
183  // get the 'label' field
184  std::uint32_t label;
185  memcpy (&label, reinterpret_cast<const char*> (&point) + fields[label_idx].offset, sizeof(std::uint32_t));
186 
187  if (static_cast<int> (label) == label_num)
188  {
189  pcl::PointXYZ tmp;
190  // X Y Z
191  tmp.x = point.x;
192  tmp.y = point.y;
193  tmp.z = point.z;
194  out->push_back (tmp);
195  }
196  }
197  out->width = out->size ();
198  out->height = 1;
199  out->is_dense = false;
200  }
201 }
202 
203 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
204 template <typename PointT> void
207  float normal_radius_search,
208  float fpfh_radius_search)
209 {
213 
214  n3d.setRadiusSearch (normal_radius_search);
215  n3d.setSearchMethod (normals_tree);
216  // ---[ Estimate the point normals
217  n3d.setInputCloud (in);
218  n3d.compute (*normals);
219 
220  // Create the FPFH estimation class, and pass the input dataset+normals to it
222  fpfh.setInputCloud (in);
223  fpfh.setInputNormals (normals);
224 
226  fpfh.setSearchMethod (tree);
227  fpfh.setRadiusSearch (fpfh_radius_search);
228  // Compute the features
229  fpfh.compute (*out);
230 }
231 
232 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
233 template <typename PointT> void
236  int k)
237 {
238  pcl::Kmeans kmeans (static_cast<int> (in->size ()), 33);
239  kmeans.setClusterSize (k);
240 
241  // add points to the clustering
242  for (const auto &point : in->points)
243  {
244  std::vector<float> data (33);
245  for (int idx = 0; idx < 33; idx++)
246  data[idx] = point.histogram[idx];
247  kmeans.addDataPoint (data);
248  }
249 
250  // k-means clustering
251  kmeans.kMeans ();
252 
253  // get the cluster centroids
254  pcl::Kmeans::Centroids centroids = kmeans.get_centroids ();
255 
256  // initialize output cloud
257  out->width = centroids.size ();
258  out->height = 1;
259  out->is_dense = false;
260  out->points.resize (static_cast<int> (centroids.size ()));
261  // copy cluster centroids into feature cloud
262  for (std::size_t i = 0; i < centroids.size (); i++)
263  {
264  pcl::FPFHSignature33 point;
265  for (int idx = 0; idx < 33; idx++)
266  point.histogram[idx] = centroids[i][idx];
267  (*out)[i] = point;
268  }
269 }
270 
271 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
272 template <typename PointT> void
275  pcl::Indices &indi,
276  std::vector<float> &dist)
277 {
278  // estimate the total number of row's needed
279  int n_row = 0;
280  for (const auto &trained_feature : trained_features)
281  n_row += static_cast<int> (trained_feature->size ());
282 
283  // Convert data into FLANN format
284  int n_col = 33;
285  flann::Matrix<float> data (new float[n_row * n_col], n_row, n_col);
286  for (std::size_t k = 0; k < trained_features.size (); k++)
287  {
288  pcl::PointCloud<pcl::FPFHSignature33>::Ptr hist = trained_features[k];
289  const auto c = hist->size ();
290  for (std::size_t i = 0; i < c; ++i)
291  for (std::size_t j = 0; j < data.cols; ++j)
292  data[(k * c) + i][j] = (*hist)[i].histogram[j];
293  }
294 
295  // build kd-tree given the training features
297  index = new flann::Index<flann::ChiSquareDistance<float> > (data, flann::LinearIndexParams ());
298  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::LinearIndexParams ());
299  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KMeansIndexParams (5, -1));
300  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KDTreeIndexParams (4));
301  index->buildIndex ();
302 
303  int k = 1;
304  indi.resize (query_features->size ());
305  dist.resize (query_features->size ());
306  // Query all points
307  for (std::size_t i = 0; i < query_features->size (); i++)
308  {
309  // Query point
310  flann::Matrix<float> p = flann::Matrix<float>(new float[n_col], 1, n_col);
311  memcpy (&p.ptr ()[0], (*query_features)[i].histogram, p.cols * p.rows * sizeof (float));
312 
313  flann::Matrix<int> indices (new int[k], 1, k);
314  flann::Matrix<float> distances (new float[k], 1, k);
315  index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
316 
317  indi[i] = indices[0][0];
318  dist[i] = distances[0][0];
319 
320  delete[] p.ptr ();
321  }
322 
323  //std::cout << "kdtree size: " << index->size () << std::endl;
324 
325  delete[] data.ptr ();
326 }
327 
328 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
329 template <typename PointT> void
331  std::vector<float> &dist,
332  int n_feature_means,
333  float feature_threshold,
335 
336 {
337  float nfm = static_cast<float> (n_feature_means);
338  for (std::size_t i = 0; i < out->size (); i++)
339  {
340  if (dist[i] < feature_threshold)
341  {
342  float l = static_cast<float> (indi[i]) / nfm;
343  float intpart;
344  //float fractpart = std::modf (l , &intpart);
345  std::modf (l , &intpart);
346  int label = static_cast<int> (intpart);
347 
348  (*out)[i].label = label+2;
349  }
350  }
351 }
352 
353 
354 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
355 template <typename PointT> void
357 {
358  // convert cloud into cloud with XYZ
360  convertCloud (input_cloud_, tmp_cloud);
361 
362  // compute FPFH feature histograms for all point of the input point cloud
364  computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
365 
366  //PCL_INFO ("Number of input cloud features: %d\n", static_cast<int> (feature->size ()));
367 
368  // use k-means to cluster the features
369  kmeansClustering (feature, output, cluster_size_);
370 }
371 
372 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
373 template <typename PointT> void
375  std::vector<pcl::PointCloud<pcl::FPFHSignature33>, Eigen::aligned_allocator<pcl::PointCloud<pcl::FPFHSignature33> > > &output)
376 {
377  // find clusters
378  std::vector<int> cluster_numbers;
379  findClusters (input_cloud_, cluster_numbers);
380  std::cout << "cluster numbers: ";
381  for (const int &cluster_number : cluster_numbers)
382  std::cout << cluster_number << " ";
383  std::cout << std::endl;
384 
385  for (const int &cluster_number : cluster_numbers)
386  {
387  // extract all points with the same label number
389  getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
390 
391  // compute FPFH feature histograms for all point of the input point cloud
393  computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
394 
395  // use k-means to cluster the features
397  kmeansClustering (feature, kmeans_feature, cluster_size_);
398 
399  output.push_back (*kmeans_feature);
400  }
401 }
402 
403 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
404 template <typename PointT> void
406 {
407  if (!trained_features_.empty ())
408  {
409  // convert cloud into cloud with XYZ
411  convertCloud (input_cloud_, tmp_cloud);
412 
413  // compute FPFH feature histograms for all point of the input point cloud
415  computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
416 
417  // query the distances from the input data features to all trained features
418  Indices indices;
419  std::vector<float> distance;
420  queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
421 
422  // assign a label to each point of the input point cloud
423  const auto n_feature_means = trained_features_[0]->size ();
424  convertCloud (input_cloud_, out);
425  assignLabels (indices, distance, n_feature_means, feature_threshold_, out);
426  //std::cout << "Assign labels - DONE" << std::endl;
427  }
428  else
429  PCL_ERROR ("no training features set \n");
430 }
431 
432 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
433 #define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier<T>;
434 
435 #endif // PCL_UNARY_CLASSIFIER_HPP_
FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud d...
Definition: fpfh.h:79
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: feature.h:345
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition: feature.h:201
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: feature.h:167
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
K-means clustering.
Definition: kmeans.h:55
Centroids get_centroids()
Definition: kmeans.h:147
void addDataPoint(Point &data_point)
Definition: kmeans.h:116
void setClusterSize(unsigned int k)
This method sets the k-means cluster size.
Definition: kmeans.h:84
std::vector< Point > Centroids
Definition: kmeans.h:71
void kMeans()
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition: normal_3d.h:244
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:332
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:652
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:395
void segment(pcl::PointCloud< pcl::PointXYZRGBL >::Ptr &out)
void setInputCloud(typename pcl::PointCloud< PointT >::Ptr input_cloud)
This method sets the input cloud.
void train(pcl::PointCloud< pcl::FPFHSignature33 >::Ptr &output)
void queryFeatureDistances(std::vector< pcl::PointCloud< pcl::FPFHSignature33 >::Ptr > &trained_features, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr query_features, pcl::Indices &indi, std::vector< float > &dist)
void assignLabels(pcl::Indices &indi, std::vector< float > &dist, int n_feature_means, float feature_threshold, pcl::PointCloud< pcl::PointXYZRGBL >::Ptr out)
void computeFPFH(pcl::PointCloud< pcl::PointXYZ >::Ptr in, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr out, float normal_radius_search, float fpfh_radius_search)
UnaryClassifier()
Constructor that sets default values for member variables.
void findClusters(typename pcl::PointCloud< PointT >::Ptr in, std::vector< int > &cluster_numbers)
~UnaryClassifier()
This destructor destroys the cloud...
void trainWithLabel(std::vector< pcl::PointCloud< pcl::FPFHSignature33 >, Eigen::aligned_allocator< pcl::PointCloud< pcl::FPFHSignature33 > > > &output)
void getCloudWithLabel(typename pcl::PointCloud< PointT >::Ptr in, pcl::PointCloud< pcl::PointXYZ >::Ptr out, int label_num)
void convertCloud(typename pcl::PointCloud< PointT >::Ptr in, pcl::PointCloud< pcl::PointXYZ >::Ptr out)
void kmeansClustering(pcl::PointCloud< pcl::FPFHSignature33 >::Ptr in, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr out, int k)
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
std::uint32_t label
A point structure representing the Fast Point Feature Histogram (FPFH).
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.