Point Cloud Library (PCL)  1.8.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 <pcl/segmentation/unary_classifier.h>
45 #include <pcl/common/io.h>
46 #include <pcl/kdtree/flann.h>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointT>
51  input_cloud_ (new pcl::PointCloud<PointT>),
52  label_field_ (false),
53  normal_radius_search_ (0.01f),
54  fpfh_radius_search_ (0.05f),
55  feature_threshold_ (5.0)
56 {
57 }
58 
59 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
60 template <typename PointT>
62 {
63 }
64 
65 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
66 template <typename PointT> void
68 {
69  if (input_cloud_ != NULL)
70  input_cloud_.reset ();
71 
72  input_cloud_ = input_cloud;
73 
75  std::vector<pcl::PCLPointField> fields;
76 
77  int label_index = -1;
78  label_index = pcl::getFieldIndex (point, "label", fields);
79 
80  if (label_index != -1)
81  label_field_ = true;
82 }
83 
84 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
85 template <typename PointT> void
88 {
89  // resize points of output cloud
90  out->points.resize (in->points.size ());
91  out->width = static_cast<int> (out->points.size ());
92  out->height = 1;
93  out->is_dense = false;
94 
95  for (size_t i = 0; i < in->points.size (); i++)
96  {
97  pcl::PointXYZ point;
98  // fill X Y Z
99  point.x = in->points[i].x;
100  point.y = in->points[i].y;
101  point.z = in->points[i].z;
102  out->points[i] = point;
103  }
104 }
105 
106 template <typename PointT> void
109 {
110  // TODO:: check if input cloud has RGBA information and insert into the cloud
111 
112  // resize points of output cloud
113  out->points.resize (in->points.size ());
114  out->width = static_cast<int> (out->points.size ());
115  out->height = 1;
116  out->is_dense = false;
117 
118  for (size_t i = 0; i < in->points.size (); i++)
119  {
120  pcl::PointXYZRGBL point;
121  // X Y Z R G B L
122  point.x = in->points[i].x;
123  point.y = in->points[i].y;
124  point.z = in->points[i].z;
125  //point.rgba = in->points[i].rgba;
126  point.label = 1;
127  out->points[i] = point;
128  }
129 }
130 
131 
132 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
133 template <typename PointT> void
135  std::vector<int> &cluster_numbers)
136 {
137  // find the 'label' field index
138  std::vector <pcl::PCLPointField> fields;
139  int label_idx = -1;
141  label_idx = pcl::getFieldIndex (point, "label", fields);
142 
143  if (label_idx != -1)
144  {
145  for (size_t i = 0; i < in->points.size (); i++)
146  {
147  // get the 'label' field
148  uint32_t label;
149  memcpy (&label, reinterpret_cast<char*> (&in->points[i]) + fields[label_idx].offset, sizeof(uint32_t));
150 
151  // check if label exist
152  bool exist = false;
153  for (size_t j = 0; j < cluster_numbers.size (); j++)
154  {
155  if (static_cast<uint32_t> (cluster_numbers[j]) == label)
156  {
157  exist = true;
158  break;
159  }
160  }
161  if (!exist)
162  cluster_numbers.push_back (label);
163  }
164  }
165 }
166 
167 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
168 template <typename PointT> void
171  int label_num)
172 {
173  // find the 'label' field index
174  std::vector <pcl::PCLPointField> fields;
175  int label_idx = -1;
177  label_idx = pcl::getFieldIndex (point, "label", fields);
178 
179  if (label_idx != -1)
180  {
181  for (size_t i = 0; i < in->points.size (); i++)
182  {
183  // get the 'label' field
184  uint32_t label;
185  memcpy (&label, reinterpret_cast<char*> (&in->points[i]) + fields[label_idx].offset, sizeof(uint32_t));
186 
187  if (static_cast<int> (label) == label_num)
188  {
189  pcl::PointXYZ point;
190  // X Y Z
191  point.x = in->points[i].x;
192  point.y = in->points[i].y;
193  point.z = in->points[i].z;
194  out->points.push_back (point);
195  }
196  }
197  out->width = static_cast<int> (out->points.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->points.size ()), 33);
239  kmeans.setClusterSize (k);
240 
241  // add points to the clustering
242  for (size_t i = 0; i < in->points.size (); i++)
243  {
244  std::vector<float> data (33);
245  for (int idx = 0; idx < 33; idx++)
246  data[idx] = in->points[i].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 = static_cast<int> (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 (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->points[i] = point;
268  }
269 }
270 
271 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
272 template <typename PointT> void
275  std::vector<int> &indi,
276  std::vector<float> &dist)
277 {
278  // estimate the total number of row's needed
279  int n_row = 0;
280  for (size_t i = 0; i < trained_features.size (); i++)
281  n_row += static_cast<int> (trained_features[i]->points.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 (size_t k = 0; k < trained_features.size (); k++)
287  {
288  pcl::PointCloud<pcl::FPFHSignature33>::Ptr hist = trained_features[k];
289  size_t c = hist->points.size ();
290  for (size_t i = 0; i < c; ++i)
291  for (size_t j = 0; j < data.cols; ++j)
292  data[(k * c) + i][j] = hist->points[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->points.size ());
305  dist.resize (query_features->points.size ());
306  // Query all points
307  for (size_t i = 0; i < query_features->points.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->points[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 (size_t i = 0; i < out->points.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 = modf (l , &intpart);
345  std::modf (l , &intpart);
346  int label = static_cast<int> (intpart);
347 
348  out->points[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
365 
366  //PCL_INFO ("Number of input cloud features: %d\n", static_cast<int> (feature->points.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 (size_t i = 0; i < cluster_numbers.size (); i++)
382  std::cout << cluster_numbers[i] << " ";
383  std::cout << std::endl;
384 
385  for (size_t i = 0; i < cluster_numbers.size (); i++)
386  {
387  // extract all points with the same label number
389  getCloudWithLabel (input_cloud_, label_cloud, cluster_numbers[i]);
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_.size () > 0)
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  std::vector<int> 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  int n_feature_means = static_cast<int> (trained_features_[0]->points.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_
std::vector< Point > Centroids
Definition: kmeans.h:92
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
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:59
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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:199
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:415
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void setClusterSize(unsigned int k)
This method sets the k-means cluster size.
Definition: kmeans.h:107
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:79
void kmeansClustering(pcl::PointCloud< pcl::FPFHSignature33 >::Ptr in, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr out, int k)
Centroids get_centroids()
Definition: kmeans.h:168
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.
FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud d...
Definition: fpfh.h:80
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
void assignLabels(std::vector< int > &indi, std::vector< float > &dist, int n_feature_means, float feature_threshold, pcl::PointCloud< pcl::PointXYZRGBL >::Ptr out)
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: normal_3d.h:290
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:66
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
void kMeans()
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:189
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 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)