Point Cloud Library (PCL)  1.15.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/features/normal_3d.h> // for NormalEstimation
50 #include <pcl/segmentation/unary_classifier.h>
51 #include <pcl/common/io.h>
52 #include <pcl/search/kdtree.h> // for KdTree
53 
54 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
55 template <typename PointT>
57 
58 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
59 template <typename PointT>
61 
62 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
63 template <typename PointT> void
65 {
66  input_cloud_ = input_cloud;
67 
69  std::vector<pcl::PCLPointField> fields;
70 
71  int label_index = -1;
72  label_index = pcl::getFieldIndex<PointT> ("label", fields);
73 
74  if (label_index != -1)
75  label_field_ = true;
76 }
77 
78 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointT> void
82 {
83  // resize points of output cloud
84  out->points.resize (in->size ());
85  out->width = out->size ();
86  out->height = 1;
87  out->is_dense = false;
88 
89  for (std::size_t i = 0; i < in->size (); i++)
90  {
91  pcl::PointXYZ point;
92  // fill X Y Z
93  point.x = (*in)[i].x;
94  point.y = (*in)[i].y;
95  point.z = (*in)[i].z;
96  (*out)[i] = point;
97  }
98 }
99 
100 template <typename PointT> void
103 {
104  // TODO:: check if input cloud has RGBA information and insert into the cloud
105 
106  // resize points of output cloud
107  out->points.resize (in->size ());
108  out->width = out->size ();
109  out->height = 1;
110  out->is_dense = false;
111 
112  for (std::size_t i = 0; i < in->size (); i++)
113  {
114  pcl::PointXYZRGBL point;
115  // X Y Z R G B L
116  point.x = (*in)[i].x;
117  point.y = (*in)[i].y;
118  point.z = (*in)[i].z;
119  //point.rgba = (*in)[i].rgba;
120  point.label = 1;
121  (*out)[i] = point;
122  }
123 }
124 
125 
126 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
127 template <typename PointT> void
129  std::vector<int> &cluster_numbers)
130 {
131  // find the 'label' field index
132  std::vector <pcl::PCLPointField> fields;
133  const int label_idx = pcl::getFieldIndex<PointT> ("label", fields);
134 
135  if (label_idx != -1)
136  {
137  for (const auto& point: *in)
138  {
139  // get the 'label' field
140  std::uint32_t label;
141  memcpy (&label, reinterpret_cast<const char*> (&point) + fields[label_idx].offset, sizeof(std::uint32_t));
142 
143  // check if label exist
144  bool exist = false;
145  for (const int &cluster_number : cluster_numbers)
146  {
147  if (static_cast<std::uint32_t> (cluster_number) == label)
148  {
149  exist = true;
150  break;
151  }
152  }
153  if (!exist)
154  cluster_numbers.push_back (label);
155  }
156  }
157 }
158 
159 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
160 template <typename PointT> void
163  int label_num)
164 {
165  // find the 'label' field index
166  std::vector <pcl::PCLPointField> fields;
167  int label_idx = -1;
168  label_idx = pcl::getFieldIndex<PointT> ("label", fields);
169 
170  if (label_idx != -1)
171  {
172  for (const auto& point : (*in))
173  {
174  // get the 'label' field
175  std::uint32_t label;
176  memcpy (&label, reinterpret_cast<const char*> (&point) + fields[label_idx].offset, sizeof(std::uint32_t));
177 
178  if (static_cast<int> (label) == label_num)
179  {
180  pcl::PointXYZ tmp;
181  // X Y Z
182  tmp.x = point.x;
183  tmp.y = point.y;
184  tmp.z = point.z;
185  out->push_back (tmp);
186  }
187  }
188  out->width = out->size ();
189  out->height = 1;
190  out->is_dense = false;
191  }
192 }
193 
194 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
195 template <typename PointT> void
198  float normal_radius_search,
199  float fpfh_radius_search)
200 {
204 
205  n3d.setRadiusSearch (normal_radius_search);
206  n3d.setSearchMethod (normals_tree);
207  // ---[ Estimate the point normals
208  n3d.setInputCloud (in);
209  n3d.compute (*normals);
210 
211  // Create the FPFH estimation class, and pass the input dataset+normals to it
213  fpfh.setInputCloud (in);
214  fpfh.setInputNormals (normals);
215 
217  fpfh.setSearchMethod (tree);
218  fpfh.setRadiusSearch (fpfh_radius_search);
219  // Compute the features
220  fpfh.compute (*out);
221 }
222 
223 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
224 template <typename PointT> void
227  int k)
228 {
229  pcl::Kmeans kmeans (static_cast<int> (in->size ()), 33);
230  kmeans.setClusterSize (k);
231 
232  // add points to the clustering
233  for (const auto &point : in->points)
234  {
235  std::vector<float> data (33);
236  for (int idx = 0; idx < 33; idx++)
237  data[idx] = point.histogram[idx];
238  kmeans.addDataPoint (data);
239  }
240 
241  // k-means clustering
242  kmeans.kMeans ();
243 
244  // get the cluster centroids
245  pcl::Kmeans::Centroids centroids = kmeans.get_centroids ();
246 
247  // initialize output cloud
248  out->width = centroids.size ();
249  out->height = 1;
250  out->is_dense = false;
251  out->points.resize (static_cast<int> (centroids.size ()));
252  // copy cluster centroids into feature cloud
253  for (std::size_t i = 0; i < centroids.size (); i++)
254  {
255  pcl::FPFHSignature33 point;
256  for (int idx = 0; idx < 33; idx++)
257  point.histogram[idx] = centroids[i][idx];
258  (*out)[i] = point;
259  }
260 }
261 
262 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
263 template <typename PointT> void
266  pcl::Indices &indi,
267  std::vector<float> &dist)
268 {
269  // estimate the total number of row's needed
270  int n_row = 0;
271  for (const auto &trained_feature : trained_features)
272  n_row += static_cast<int> (trained_feature->size ());
273 
274  // Convert data into FLANN format
275  int n_col = 33;
276  flann::Matrix<float> data (new float[n_row * n_col], n_row, n_col);
277  for (std::size_t k = 0; k < trained_features.size (); k++)
278  {
279  pcl::PointCloud<pcl::FPFHSignature33>::Ptr hist = trained_features[k];
280  const auto c = hist->size ();
281  for (std::size_t i = 0; i < c; ++i)
282  for (std::size_t j = 0; j < data.cols; ++j)
283  data[(k * c) + i][j] = (*hist)[i].histogram[j];
284  }
285 
286  // build kd-tree given the training features
288  index = new flann::Index<flann::ChiSquareDistance<float> > (data, flann::LinearIndexParams ());
289  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::LinearIndexParams ());
290  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KMeansIndexParams (5, -1));
291  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KDTreeIndexParams (4));
292  index->buildIndex ();
293 
294  int k = 1;
295  indi.resize (query_features->size ());
296  dist.resize (query_features->size ());
297  // Query all points
298  for (std::size_t i = 0; i < query_features->size (); i++)
299  {
300  // Query point
301  flann::Matrix<float> p = flann::Matrix<float>(new float[n_col], 1, n_col);
302  std::copy((*query_features)[i].histogram, (*query_features)[i].histogram + n_col, p.ptr());
303 
304  flann::Matrix<int> indices (new int[k], 1, k);
305  flann::Matrix<float> distances (new float[k], 1, k);
306  index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
307 
308  indi[i] = indices[0][0];
309  dist[i] = distances[0][0];
310 
311  delete[] p.ptr ();
312  }
313 
314  //std::cout << "kdtree size: " << index->size () << std::endl;
315 
316  delete[] data.ptr ();
317 }
318 
319 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
320 template <typename PointT> void
322  std::vector<float> &dist,
323  int n_feature_means,
324  float feature_threshold,
326 
327 {
328  float nfm = static_cast<float> (n_feature_means);
329  for (std::size_t i = 0; i < out->size (); i++)
330  {
331  if (dist[i] < feature_threshold)
332  {
333  float l = static_cast<float> (indi[i]) / nfm;
334  float intpart;
335  //float fractpart = std::modf (l , &intpart);
336  std::modf (l , &intpart);
337  int label = static_cast<int> (intpart);
338 
339  (*out)[i].label = label+2;
340  }
341  }
342 }
343 
344 
345 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
346 template <typename PointT> void
348 {
349  // convert cloud into cloud with XYZ
351  convertCloud (input_cloud_, tmp_cloud);
352 
353  // compute FPFH feature histograms for all point of the input point cloud
355  computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
356 
357  //PCL_INFO ("Number of input cloud features: %d\n", static_cast<int> (feature->size ()));
358 
359  // use k-means to cluster the features
360  kmeansClustering (feature, output, cluster_size_);
361 }
362 
363 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
364 template <typename PointT> void
366  std::vector<pcl::PointCloud<pcl::FPFHSignature33>, Eigen::aligned_allocator<pcl::PointCloud<pcl::FPFHSignature33> > > &output)
367 {
368  // find clusters
369  std::vector<int> cluster_numbers;
370  findClusters (input_cloud_, cluster_numbers);
371  std::cout << "cluster numbers: ";
372  for (const int &cluster_number : cluster_numbers)
373  std::cout << cluster_number << " ";
374  std::cout << std::endl;
375 
376  for (const int &cluster_number : cluster_numbers)
377  {
378  // extract all points with the same label number
380  getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
381 
382  // compute FPFH feature histograms for all point of the input point cloud
384  computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
385 
386  // use k-means to cluster the features
388  kmeansClustering (feature, kmeans_feature, cluster_size_);
389 
390  output.push_back (*kmeans_feature);
391  }
392 }
393 
394 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
395 template <typename PointT> void
397 {
398  if (!trained_features_.empty ())
399  {
400  // convert cloud into cloud with XYZ
402  convertCloud (input_cloud_, tmp_cloud);
403 
404  // compute FPFH feature histograms for all point of the input point cloud
406  computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
407 
408  // query the distances from the input data features to all trained features
409  Indices indices;
410  std::vector<float> distance;
411  queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
412 
413  // assign a label to each point of the input point cloud
414  const auto n_feature_means = trained_features_[0]->size ();
415  convertCloud (input_cloud_, out);
416  assignLabels (indices, distance, n_feature_means, feature_threshold_, out);
417  //std::cout << "Assign labels - DONE" << std::endl;
418  }
419  else
420  PCL_ERROR ("no training features set \n");
421 }
422 
423 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
424 #define PCL_INSTANTIATE_UnaryClassifier(T) template class PCL_EXPORTS pcl::UnaryClassifier<T>;
425 
426 #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:339
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:198
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: feature.h:164
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:195
K-means clustering.
Definition: kmeans.h:55
Centroids get_centroids()
Definition: kmeans.h:144
void addDataPoint(Point &data_point)
Definition: kmeans.h:113
void setClusterSize(unsigned int k)
This method sets the k-means cluster size.
Definition: kmeans.h:81
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:328
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:174
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:664
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:404
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:399
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:401
std::size_t size() const
Definition: point_cloud.h:444
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:414
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:396
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
PCL_ADD_POINT4D PCL_ADD_RGB std::uint32_t label
A point structure representing the Fast Point Feature Histogram (FPFH).
A point structure representing Euclidean xyz coordinates.