Point Cloud Library (PCL)  1.11.1-dev
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;
178  label_idx = pcl::getFieldIndex<PointT> ("label", fields);
179 
180  if (label_idx != -1)
181  {
182  for (std::size_t i = 0; i < in->size (); i++)
183  {
184  // get the 'label' field
185  std::uint32_t label;
186  memcpy (&label, reinterpret_cast<char*> (&(*in)[i]) + fields[label_idx].offset, sizeof(std::uint32_t));
187 
188  if (static_cast<int> (label) == label_num)
189  {
190  pcl::PointXYZ point;
191  // X Y Z
192  point.x = (*in)[i].x;
193  point.y = (*in)[i].y;
194  point.z = (*in)[i].z;
195  out->points.push_back (point);
196  }
197  }
198  out->width = out->size ();
199  out->height = 1;
200  out->is_dense = false;
201  }
202 }
203 
204 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
205 template <typename PointT> void
208  float normal_radius_search,
209  float fpfh_radius_search)
210 {
214 
215  n3d.setRadiusSearch (normal_radius_search);
216  n3d.setSearchMethod (normals_tree);
217  // ---[ Estimate the point normals
218  n3d.setInputCloud (in);
219  n3d.compute (*normals);
220 
221  // Create the FPFH estimation class, and pass the input dataset+normals to it
223  fpfh.setInputCloud (in);
224  fpfh.setInputNormals (normals);
225 
227  fpfh.setSearchMethod (tree);
228  fpfh.setRadiusSearch (fpfh_radius_search);
229  // Compute the features
230  fpfh.compute (*out);
231 }
232 
233 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
234 template <typename PointT> void
237  int k)
238 {
239  pcl::Kmeans kmeans (static_cast<int> (in->size ()), 33);
240  kmeans.setClusterSize (k);
241 
242  // add points to the clustering
243  for (const auto &point : in->points)
244  {
245  std::vector<float> data (33);
246  for (int idx = 0; idx < 33; idx++)
247  data[idx] = point.histogram[idx];
248  kmeans.addDataPoint (data);
249  }
250 
251  // k-means clustering
252  kmeans.kMeans ();
253 
254  // get the cluster centroids
255  pcl::Kmeans::Centroids centroids = kmeans.get_centroids ();
256 
257  // initialize output cloud
258  out->width = centroids.size ();
259  out->height = 1;
260  out->is_dense = false;
261  out->points.resize (static_cast<int> (centroids.size ()));
262  // copy cluster centroids into feature cloud
263  for (std::size_t i = 0; i < centroids.size (); i++)
264  {
265  pcl::FPFHSignature33 point;
266  for (int idx = 0; idx < 33; idx++)
267  point.histogram[idx] = centroids[i][idx];
268  (*out)[i] = point;
269  }
270 }
271 
272 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
273 template <typename PointT> void
276  std::vector<int> &indi,
277  std::vector<float> &dist)
278 {
279  // estimate the total number of row's needed
280  int n_row = 0;
281  for (const auto &trained_feature : trained_features)
282  n_row += static_cast<int> (trained_feature->size ());
283 
284  // Convert data into FLANN format
285  int n_col = 33;
286  flann::Matrix<float> data (new float[n_row * n_col], n_row, n_col);
287  for (std::size_t k = 0; k < trained_features.size (); k++)
288  {
289  pcl::PointCloud<pcl::FPFHSignature33>::Ptr hist = trained_features[k];
290  const auto c = hist->size ();
291  for (std::size_t i = 0; i < c; ++i)
292  for (std::size_t j = 0; j < data.cols; ++j)
293  data[(k * c) + i][j] = (*hist)[i].histogram[j];
294  }
295 
296  // build kd-tree given the training features
298  index = new flann::Index<flann::ChiSquareDistance<float> > (data, flann::LinearIndexParams ());
299  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::LinearIndexParams ());
300  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KMeansIndexParams (5, -1));
301  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KDTreeIndexParams (4));
302  index->buildIndex ();
303 
304  int k = 1;
305  indi.resize (query_features->size ());
306  dist.resize (query_features->size ());
307  // Query all points
308  for (std::size_t i = 0; i < query_features->size (); i++)
309  {
310  // Query point
311  flann::Matrix<float> p = flann::Matrix<float>(new float[n_col], 1, n_col);
312  memcpy (&p.ptr ()[0], (*query_features)[i].histogram, p.cols * p.rows * sizeof (float));
313 
314  flann::Matrix<int> indices (new int[k], 1, k);
315  flann::Matrix<float> distances (new float[k], 1, k);
316  index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
317 
318  indi[i] = indices[0][0];
319  dist[i] = distances[0][0];
320 
321  delete[] p.ptr ();
322  }
323 
324  //std::cout << "kdtree size: " << index->size () << std::endl;
325 
326  delete[] data.ptr ();
327 }
328 
329 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
330 template <typename PointT> void
332  std::vector<float> &dist,
333  int n_feature_means,
334  float feature_threshold,
336 
337 {
338  float nfm = static_cast<float> (n_feature_means);
339  for (std::size_t i = 0; i < out->size (); i++)
340  {
341  if (dist[i] < feature_threshold)
342  {
343  float l = static_cast<float> (indi[i]) / nfm;
344  float intpart;
345  //float fractpart = std::modf (l , &intpart);
346  std::modf (l , &intpart);
347  int label = static_cast<int> (intpart);
348 
349  (*out)[i].label = label+2;
350  }
351  }
352 }
353 
354 
355 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
356 template <typename PointT> void
358 {
359  // convert cloud into cloud with XYZ
361  convertCloud (input_cloud_, tmp_cloud);
362 
363  // compute FPFH feature histograms for all point of the input point cloud
365  computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
366 
367  //PCL_INFO ("Number of input cloud features: %d\n", static_cast<int> (feature->size ()));
368 
369  // use k-means to cluster the features
370  kmeansClustering (feature, output, cluster_size_);
371 }
372 
373 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
374 template <typename PointT> void
376  std::vector<pcl::PointCloud<pcl::FPFHSignature33>, Eigen::aligned_allocator<pcl::PointCloud<pcl::FPFHSignature33> > > &output)
377 {
378  // find clusters
379  std::vector<int> cluster_numbers;
380  findClusters (input_cloud_, cluster_numbers);
381  std::cout << "cluster numbers: ";
382  for (const int &cluster_number : cluster_numbers)
383  std::cout << cluster_number << " ";
384  std::cout << std::endl;
385 
386  for (const int &cluster_number : cluster_numbers)
387  {
388  // extract all points with the same label number
390  getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
391 
392  // compute FPFH feature histograms for all point of the input point cloud
394  computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
395 
396  // use k-means to cluster the features
398  kmeansClustering (feature, kmeans_feature, cluster_size_);
399 
400  output.push_back (*kmeans_feature);
401  }
402 }
403 
404 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
405 template <typename PointT> void
407 {
408  if (!trained_features_.empty ())
409  {
410  // convert cloud into cloud with XYZ
412  convertCloud (input_cloud_, tmp_cloud);
413 
414  // compute FPFH feature histograms for all point of the input point cloud
416  computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
417 
418  // query the distances from the input data features to all trained features
419  Indices indices;
420  std::vector<float> distance;
421  queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
422 
423  // assign a label to each point of the input point cloud
424  const auto n_feature_means = trained_features_[0]->size ();
425  convertCloud (input_cloud_, out);
426  assignLabels (indices, distance, n_feature_means, feature_threshold_, out);
427  //std::cout << "Assign labels - DONE" << std::endl;
428  }
429  else
430  PCL_ERROR ("no training features set \n");
431 }
432 
433 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
434 #define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier<T>;
435 
436 #endif // PCL_UNARY_CLASSIFIER_HPP_
pcl
Definition: convolution.h:46
pcl::UnaryClassifier::queryFeatureDistances
void queryFeatureDistances(std::vector< pcl::PointCloud< pcl::FPFHSignature33 >::Ptr > &trained_features, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr query_features, std::vector< int > &indi, std::vector< float > &dist)
Definition: unary_classifier.hpp:274
pcl::Kmeans::Centroids
std::vector< Point > Centroids
Definition: kmeans.h:71
pcl::NormalEstimation
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition: normal_3d.h:243
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:389
pcl::UnaryClassifier::computeFPFH
void computeFPFH(pcl::PointCloud< pcl::PointXYZ >::Ptr in, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr out, float normal_radius_search, float fpfh_radius_search)
Definition: unary_classifier.hpp:206
pcl::UnaryClassifier::UnaryClassifier
UnaryClassifier()
Constructor that sets default values for member variables.
Definition: unary_classifier.hpp:54
pcl::UnaryClassifier::segment
void segment(pcl::PointCloud< pcl::PointXYZRGBL >::Ptr &out)
Definition: unary_classifier.hpp:406
pcl::PointXYZRGBL
Definition: point_types.hpp:659
pcl::Kmeans
K-means clustering.
Definition: kmeans.h:55
pcl::FPFHSignature33
A point structure representing the Fast Point Feature Histogram (FPFH).
Definition: point_types.hpp:1476
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PCLBase< PointInT >::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::search::KdTree::Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
flann::Index
Definition: kdtree_flann.h:52
pcl::UnaryClassifier::findClusters
void findClusters(typename pcl::PointCloud< PointT >::Ptr in, std::vector< int > &cluster_numbers)
Definition: unary_classifier.hpp:135
pcl::NormalEstimation::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:332
pcl::UnaryClassifier::getCloudWithLabel
void getCloudWithLabel(typename pcl::PointCloud< PointT >::Ptr in, pcl::PointCloud< pcl::PointXYZ >::Ptr out, int label_num)
Definition: unary_classifier.hpp:170
flann::Matrix
Definition: flann_search.h:50
pcl::Kmeans::get_centroids
Centroids get_centroids()
Definition: kmeans.h:147
pcl::Kmeans::kMeans
void kMeans()
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition: point_types.hpp:300
pcl::FPFHEstimation
FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud d...
Definition: fpfh.h:78
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
pcl::FeatureFromNormals< PointInT, PointNT, pcl::FPFHSignature33 >::setInputNormals
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
pcl::UnaryClassifier::train
void train(pcl::PointCloud< pcl::FPFHSignature33 >::Ptr &output)
Definition: unary_classifier.hpp:357
pcl::FPFHSignature33::histogram
float histogram[33]
Definition: point_types.hpp:1478
pcl::Kmeans::addDataPoint
void addDataPoint(Point &data_point)
Definition: kmeans.h:116
pcl::PointCloud::is_dense
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:397
pcl::_PointXYZRGBL::label
std::uint32_t label
Definition: point_types.hpp:592
pcl::UnaryClassifier::~UnaryClassifier
~UnaryClassifier()
This destructor destroys the cloud...
Definition: unary_classifier.hpp:65
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::UnaryClassifier::assignLabels
void assignLabels(std::vector< int > &indi, std::vector< float > &dist, int n_feature_means, float feature_threshold, pcl::PointCloud< pcl::PointXYZRGBL >::Ptr out)
Definition: unary_classifier.hpp:331
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:407
pcl::UnaryClassifier::convertCloud
void convertCloud(typename pcl::PointCloud< PointT >::Ptr in, pcl::PointCloud< pcl::PointXYZ >::Ptr out)
Definition: unary_classifier.hpp:87
pcl::Feature::setRadiusSearch
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
pcl::UnaryClassifier::kmeansClustering
void kmeansClustering(pcl::PointCloud< pcl::FPFHSignature33 >::Ptr in, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr out, int k)
Definition: unary_classifier.hpp:235
pcl::UnaryClassifier::trainWithLabel
void trainWithLabel(std::vector< pcl::PointCloud< pcl::FPFHSignature33 >, Eigen::aligned_allocator< pcl::PointCloud< pcl::FPFHSignature33 > > > &output)
Definition: unary_classifier.hpp:375
pcl::UnaryClassifier::setInputCloud
void setInputCloud(typename pcl::PointCloud< PointT >::Ptr input_cloud)
This method sets the input cloud.
Definition: unary_classifier.hpp:71
pcl::Kmeans::setClusterSize
void setClusterSize(unsigned int k)
This method sets the k-means cluster size.
Definition: kmeans.h:84
pcl::Feature::setSearchMethod
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: feature.h:167