Point Cloud Library (PCL) 1.13.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 <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
54template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
65template <typename PointT>
67
68//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
85template <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
106template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
133template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
166template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
201template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
230template <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 {
262 for (int idx = 0; idx < 33; idx++)
263 point.histogram[idx] = centroids[i][idx];
264 (*out)[i] = point;
265 }
266}
267
268//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
269template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
326template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
352template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
370template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
401template <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.