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