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