Point Cloud Library (PCL)  1.9.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/algorithms/center_chooser.h>
45 #include <flann/util/matrix.h>
46 
47 #include <pcl/segmentation/unary_classifier.h>
48 #include <pcl/common/io.h>
49 
50 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointT>
53  input_cloud_ (new pcl::PointCloud<PointT>),
54  label_field_ (false),
55  normal_radius_search_ (0.01f),
56  fpfh_radius_search_ (0.05f),
57  feature_threshold_ (5.0)
58 {
59 }
60 
61 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
62 template <typename PointT>
64 {
65 }
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 (point, "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->points.size ());
90  out->width = static_cast<int> (out->points.size ());
91  out->height = 1;
92  out->is_dense = false;
93 
94  for (size_t i = 0; i < in->points.size (); i++)
95  {
96  pcl::PointXYZ point;
97  // fill X Y Z
98  point.x = in->points[i].x;
99  point.y = in->points[i].y;
100  point.z = in->points[i].z;
101  out->points[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->points.size ());
113  out->width = static_cast<int> (out->points.size ());
114  out->height = 1;
115  out->is_dense = false;
116 
117  for (size_t i = 0; i < in->points.size (); i++)
118  {
119  pcl::PointXYZRGBL point;
120  // X Y Z R G B L
121  point.x = in->points[i].x;
122  point.y = in->points[i].y;
123  point.z = in->points[i].z;
124  //point.rgba = in->points[i].rgba;
125  point.label = 1;
126  out->points[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 (point, "label", fields);
141 
142  if (label_idx != -1)
143  {
144  for (size_t i = 0; i < in->points.size (); i++)
145  {
146  // get the 'label' field
147  uint32_t label;
148  memcpy (&label, reinterpret_cast<char*> (&in->points[i]) + fields[label_idx].offset, sizeof(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<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;
176  label_idx = pcl::getFieldIndex (point, "label", fields);
177 
178  if (label_idx != -1)
179  {
180  for (size_t i = 0; i < in->points.size (); i++)
181  {
182  // get the 'label' field
183  uint32_t label;
184  memcpy (&label, reinterpret_cast<char*> (&in->points[i]) + fields[label_idx].offset, sizeof(uint32_t));
185 
186  if (static_cast<int> (label) == label_num)
187  {
188  pcl::PointXYZ point;
189  // X Y Z
190  point.x = in->points[i].x;
191  point.y = in->points[i].y;
192  point.z = in->points[i].z;
193  out->points.push_back (point);
194  }
195  }
196  out->width = static_cast<int> (out->points.size ());
197  out->height = 1;
198  out->is_dense = false;
199  }
200 }
201 
202 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
203 template <typename PointT> void
206  float normal_radius_search,
207  float fpfh_radius_search)
208 {
212 
213  n3d.setRadiusSearch (normal_radius_search);
214  n3d.setSearchMethod (normals_tree);
215  // ---[ Estimate the point normals
216  n3d.setInputCloud (in);
217  n3d.compute (*normals);
218 
219  // Create the FPFH estimation class, and pass the input dataset+normals to it
221  fpfh.setInputCloud (in);
222  fpfh.setInputNormals (normals);
223 
225  fpfh.setSearchMethod (tree);
226  fpfh.setRadiusSearch (fpfh_radius_search);
227  // Compute the features
228  fpfh.compute (*out);
229 }
230 
231 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
232 template <typename PointT> void
235  int k)
236 {
237  pcl::Kmeans kmeans (static_cast<int> (in->points.size ()), 33);
238  kmeans.setClusterSize (k);
239 
240  // add points to the clustering
241  for (const auto &point : in->points)
242  {
243  std::vector<float> data (33);
244  for (int idx = 0; idx < 33; idx++)
245  data[idx] = point.histogram[idx];
246  kmeans.addDataPoint (data);
247  }
248 
249  // k-means clustering
250  kmeans.kMeans ();
251 
252  // get the cluster centroids
253  pcl::Kmeans::Centroids centroids = kmeans.get_centroids ();
254 
255  // initialize output cloud
256  out->width = static_cast<int> (centroids.size ());
257  out->height = 1;
258  out->is_dense = false;
259  out->points.resize (static_cast<int> (centroids.size ()));
260  // copy cluster centroids into feature cloud
261  for (size_t i = 0; i < centroids.size (); i++)
262  {
263  pcl::FPFHSignature33 point;
264  for (int idx = 0; idx < 33; idx++)
265  point.histogram[idx] = centroids[i][idx];
266  out->points[i] = point;
267  }
268 }
269 
270 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
271 template <typename PointT> void
274  std::vector<int> &indi,
275  std::vector<float> &dist)
276 {
277  // estimate the total number of row's needed
278  int n_row = 0;
279  for (const auto &trained_feature : trained_features)
280  n_row += static_cast<int> (trained_feature->points.size ());
281 
282  // Convert data into FLANN format
283  int n_col = 33;
284  flann::Matrix<float> data (new float[n_row * n_col], n_row, n_col);
285  for (size_t k = 0; k < trained_features.size (); k++)
286  {
287  pcl::PointCloud<pcl::FPFHSignature33>::Ptr hist = trained_features[k];
288  size_t c = hist->points.size ();
289  for (size_t i = 0; i < c; ++i)
290  for (size_t j = 0; j < data.cols; ++j)
291  data[(k * c) + i][j] = hist->points[i].histogram[j];
292  }
293 
294  // build kd-tree given the training features
296  index = new flann::Index<flann::ChiSquareDistance<float> > (data, flann::LinearIndexParams ());
297  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::LinearIndexParams ());
298  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KMeansIndexParams (5, -1));
299  //flann::Index<flann::ChiSquareDistance<float> > index (data, flann::KDTreeIndexParams (4));
300  index->buildIndex ();
301 
302  int k = 1;
303  indi.resize (query_features->points.size ());
304  dist.resize (query_features->points.size ());
305  // Query all points
306  for (size_t i = 0; i < query_features->points.size (); i++)
307  {
308  // Query point
309  flann::Matrix<float> p = flann::Matrix<float>(new float[n_col], 1, n_col);
310  memcpy (&p.ptr ()[0], query_features->points[i].histogram, p.cols * p.rows * sizeof (float));
311 
312  flann::Matrix<int> indices (new int[k], 1, k);
313  flann::Matrix<float> distances (new float[k], 1, k);
314  index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
315 
316  indi[i] = indices[0][0];
317  dist[i] = distances[0][0];
318 
319  delete[] p.ptr ();
320  }
321 
322  //std::cout << "kdtree size: " << index->size () << std::endl;
323 
324  delete[] data.ptr ();
325 }
326 
327 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
328 template <typename PointT> void
330  std::vector<float> &dist,
331  int n_feature_means,
332  float feature_threshold,
334 
335 {
336  float nfm = static_cast<float> (n_feature_means);
337  for (size_t i = 0; i < out->points.size (); i++)
338  {
339  if (dist[i] < feature_threshold)
340  {
341  float l = static_cast<float> (indi[i]) / nfm;
342  float intpart;
343  //float fractpart = std::modf (l , &intpart);
344  std::modf (l , &intpart);
345  int label = static_cast<int> (intpart);
346 
347  out->points[i].label = label+2;
348  }
349  }
350 }
351 
352 
353 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
354 template <typename PointT> void
356 {
357  // convert cloud into cloud with XYZ
359  convertCloud (input_cloud_, tmp_cloud);
360 
361  // compute FPFH feature histograms for all point of the input point cloud
364 
365  //PCL_INFO ("Number of input cloud features: %d\n", static_cast<int> (feature->points.size ()));
366 
367  // use k-means to cluster the features
368  kmeansClustering (feature, output, cluster_size_);
369 }
370 
371 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
372 template <typename PointT> void
374  std::vector<pcl::PointCloud<pcl::FPFHSignature33>, Eigen::aligned_allocator<pcl::PointCloud<pcl::FPFHSignature33> > > &output)
375 {
376  // find clusters
377  std::vector<int> cluster_numbers;
378  findClusters (input_cloud_, cluster_numbers);
379  std::cout << "cluster numbers: ";
380  for (const int &cluster_number : cluster_numbers)
381  std::cout << cluster_number << " ";
382  std::cout << std::endl;
383 
384  for (const int &cluster_number : cluster_numbers)
385  {
386  // extract all points with the same label number
388  getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
389 
390  // compute FPFH feature histograms for all point of the input point cloud
392  computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
393 
394  // use k-means to cluster the features
396  kmeansClustering (feature, kmeans_feature, cluster_size_);
397 
398  output.push_back (*kmeans_feature);
399  }
400 }
401 
402 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
403 template <typename PointT> void
405 {
406  if (!trained_features_.empty ())
407  {
408  // convert cloud into cloud with XYZ
410  convertCloud (input_cloud_, tmp_cloud);
411 
412  // compute FPFH feature histograms for all point of the input point cloud
414  computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
415 
416  // query the distances from the input data features to all trained features
417  std::vector<int> indices;
418  std::vector<float> distance;
419  queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
420 
421  // assign a label to each point of the input point cloud
422  int n_feature_means = static_cast<int> (trained_features_[0]->points.size ());
423  convertCloud (input_cloud_, out);
424  assignLabels (indices, distance, n_feature_means, feature_threshold_, out);
425  //std::cout << "Assign labels - DONE" << std::endl;
426  }
427  else
428  PCL_ERROR ("no training features set \n");
429 }
430 
431 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
432 #define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier<T>;
433 
434 #endif // PCL_UNARY_CLASSIFIER_HPP_
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
void addDataPoint(Point &data_point)
Definition: kmeans.h:136
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.h:58
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
std::vector< Point > Centroids
Definition: kmeans.h:92
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void convertCloud(typename pcl::PointCloud< PointT >::Ptr in, pcl::PointCloud< pcl::PointXYZ >::Ptr out)
K-means clustering.
Definition: kmeans.h:61
unsigned int cluster_size_
void train(pcl::PointCloud< pcl::FPFHSignature33 >::Ptr &output)
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:200
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:242
void findClusters(typename pcl::PointCloud< PointT >::Ptr in, std::vector< int > &cluster_numbers)
std::vector< pcl::PointCloud< pcl::FPFHSignature33 >::Ptr > trained_features_
A point structure representing the Fast Point Feature Histogram (FPFH).
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:428
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:426
void setClusterSize(unsigned int k)
This method sets the k-means cluster size.
Definition: kmeans.h:107
void kmeansClustering(pcl::PointCloud< pcl::FPFHSignature33 >::Ptr in, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr out, int k)
Centroids get_centroids()
Definition: kmeans.h:166
A point structure representing Euclidean xyz coordinates.
~UnaryClassifier()
This destructor destroys the cloud...
void setInputCloud(typename pcl::PointCloud< PointT >::Ptr input_cloud)
This method sets the input cloud.
pcl::PointCloud< PointT >::Ptr input_cloud_
Contains the input cloud.
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:78
FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud d...
Definition: fpfh.h:79
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:441
void assignLabels(std::vector< int > &indi, std::vector< float > &dist, int n_feature_means, float feature_threshold, pcl::PointCloud< pcl::PointXYZRGBL >::Ptr out)
void trainWithLabel(std::vector< pcl::PointCloud< pcl::FPFHSignature33 >, Eigen::aligned_allocator< pcl::PointCloud< pcl::FPFHSignature33 > > > &output)
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: feature.h:166
void segment(pcl::PointCloud< pcl::PointXYZRGBL >::Ptr &out)
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
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:431
void kMeans()
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:192
A point structure representing Euclidean xyz coordinates, and the RGB color.
void computeFPFH(pcl::PointCloud< pcl::PointXYZ >::Ptr in, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr out, float normal_radius_search, float fpfh_radius_search)
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)
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:331
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
Definition: feature.h:344
UnaryClassifier()
Constructor that sets default values for member variables.
void getCloudWithLabel(typename pcl::PointCloud< PointT >::Ptr in, pcl::PointCloud< pcl::PointXYZ >::Ptr out, int label_num)