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