Point Cloud Library (PCL) 1.15.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/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
57//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
58template <typename PointT>
60
61//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
62template <typename PointT> void
64{
65 input_cloud_ = input_cloud;
66
67 pcl::PointCloud <PointT> point;
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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78template <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
99template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
126template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
159template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
194template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
223template <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 {
255 for (int idx = 0; idx < 33; idx++)
256 point.histogram[idx] = centroids[i][idx];
257 (*out)[i] = point;
258 }
259}
260
261//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
262template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
319template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
345template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
363template <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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
394template <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.
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
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.