Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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/segmentation/unary_classifier.h>
50#include <pcl/common/io.h>
51
52//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
53template <typename PointT>
55 input_cloud_ (new pcl::PointCloud<PointT>),
56 label_field_ (false),
57 normal_radius_search_ (0.01f),
58 fpfh_radius_search_ (0.05f),
59 feature_threshold_ (5.0)
60{
61}
62
63//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
64template <typename PointT>
66{
67}
68
69//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70template <typename PointT> void
72{
73 input_cloud_ = input_cloud;
74
75 pcl::PointCloud <PointT> point;
76 std::vector<pcl::PCLPointField> fields;
77
78 int label_index = -1;
79 label_index = pcl::getFieldIndex<PointT> ("label", fields);
80
81 if (label_index != -1)
82 label_field_ = true;
83}
84
85//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
86template <typename PointT> void
89{
90 // resize points of output cloud
91 out->points.resize (in->size ());
92 out->width = out->size ();
93 out->height = 1;
94 out->is_dense = false;
95
96 for (std::size_t i = 0; i < in->size (); i++)
97 {
98 pcl::PointXYZ point;
99 // fill X Y Z
100 point.x = (*in)[i].x;
101 point.y = (*in)[i].y;
102 point.z = (*in)[i].z;
103 (*out)[i] = point;
104 }
105}
106
107template <typename PointT> void
110{
111 // TODO:: check if input cloud has RGBA information and insert into the cloud
112
113 // resize points of output cloud
114 out->points.resize (in->size ());
115 out->width = out->size ();
116 out->height = 1;
117 out->is_dense = false;
118
119 for (std::size_t i = 0; i < in->size (); i++)
120 {
121 pcl::PointXYZRGBL point;
122 // X Y Z R G B L
123 point.x = (*in)[i].x;
124 point.y = (*in)[i].y;
125 point.z = (*in)[i].z;
126 //point.rgba = (*in)[i].rgba;
127 point.label = 1;
128 (*out)[i] = point;
129 }
130}
131
132
133//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
134template <typename PointT> void
136 std::vector<int> &cluster_numbers)
137{
138 // find the 'label' field index
139 std::vector <pcl::PCLPointField> fields;
140 int label_idx = -1;
141 pcl::PointCloud <PointT> point;
142 label_idx = pcl::getFieldIndex<PointT> ("label", fields);
143
144 if (label_idx != -1)
145 {
146 for (const auto& point: *in)
147 {
148 // get the 'label' field
149 std::uint32_t label;
150 memcpy (&label, reinterpret_cast<const char*> (&point) + fields[label_idx].offset, sizeof(std::uint32_t));
151
152 // check if label exist
153 bool exist = false;
154 for (const int &cluster_number : cluster_numbers)
155 {
156 if (static_cast<std::uint32_t> (cluster_number) == label)
157 {
158 exist = true;
159 break;
160 }
161 }
162 if (!exist)
163 cluster_numbers.push_back (label);
164 }
165 }
166}
167
168//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
169template <typename PointT> void
172 int label_num)
173{
174 // find the 'label' field index
175 std::vector <pcl::PCLPointField> fields;
176 int label_idx = -1;
177 label_idx = pcl::getFieldIndex<PointT> ("label", fields);
178
179 if (label_idx != -1)
180 {
181 for (const auto& point : (*in))
182 {
183 // get the 'label' field
184 std::uint32_t label;
185 memcpy (&label, reinterpret_cast<const char*> (&point) + fields[label_idx].offset, sizeof(std::uint32_t));
186
187 if (static_cast<int> (label) == label_num)
188 {
189 pcl::PointXYZ tmp;
190 // X Y Z
191 tmp.x = point.x;
192 tmp.y = point.y;
193 tmp.z = point.z;
194 out->push_back (tmp);
195 }
196 }
197 out->width = out->size ();
198 out->height = 1;
199 out->is_dense = false;
200 }
201}
202
203//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
204template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
233template <typename PointT> void
236 int k)
237{
238 pcl::Kmeans kmeans (static_cast<int> (in->size ()), 33);
239 kmeans.setClusterSize (k);
240
241 // add points to the clustering
242 for (const auto &point : in->points)
243 {
244 std::vector<float> data (33);
245 for (int idx = 0; idx < 33; idx++)
246 data[idx] = point.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 = 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 (std::size_t i = 0; i < centroids.size (); i++)
263 {
265 for (int idx = 0; idx < 33; idx++)
266 point.histogram[idx] = centroids[i][idx];
267 (*out)[i] = point;
268 }
269}
270
271//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
272template <typename PointT> void
275 pcl::Indices &indi,
276 std::vector<float> &dist)
277{
278 // estimate the total number of row's needed
279 int n_row = 0;
280 for (const auto &trained_feature : trained_features)
281 n_row += static_cast<int> (trained_feature->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 (std::size_t k = 0; k < trained_features.size (); k++)
287 {
288 pcl::PointCloud<pcl::FPFHSignature33>::Ptr hist = trained_features[k];
289 const auto c = hist->size ();
290 for (std::size_t i = 0; i < c; ++i)
291 for (std::size_t j = 0; j < data.cols; ++j)
292 data[(k * c) + i][j] = (*hist)[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->size ());
305 dist.resize (query_features->size ());
306 // Query all points
307 for (std::size_t i = 0; i < query_features->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)[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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
329template <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 (std::size_t i = 0; i < out->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 = std::modf (l , &intpart);
345 std::modf (l , &intpart);
346 int label = static_cast<int> (intpart);
347
348 (*out)[i].label = label+2;
349 }
350 }
351}
352
353
354//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
355template <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
364 computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
365
366 //PCL_INFO ("Number of input cloud features: %d\n", static_cast<int> (feature->size ()));
367
368 // use k-means to cluster the features
369 kmeansClustering (feature, output, cluster_size_);
370}
371
372//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
373template <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 (const int &cluster_number : cluster_numbers)
382 std::cout << cluster_number << " ";
383 std::cout << std::endl;
384
385 for (const int &cluster_number : cluster_numbers)
386 {
387 // extract all points with the same label number
389 getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
404template <typename PointT> void
406{
407 if (!trained_features_.empty ())
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 Indices 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 const auto n_feature_means = trained_features_[0]->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_
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:345
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:201
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition feature.h:167
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:147
void addDataPoint(Point &data_point)
Definition kmeans.h:116
void setClusterSize(unsigned int k)
This method sets the k-means cluster size.
Definition kmeans.h:84
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.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::size_t size() const
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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
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.