40 #ifndef PCL_UNARY_CLASSIFIER_HPP_
41 #define PCL_UNARY_CLASSIFIER_HPP_
44 #include <pcl/segmentation/unary_classifier.h>
45 #include <pcl/common/io.h>
46 #include <pcl/kdtree/flann.h>
49 template <
typename Po
intT>
53 normal_radius_search_ (0.01f),
54 fpfh_radius_search_ (0.05f),
55 feature_threshold_ (5.0)
60 template <
typename Po
intT>
66 template <
typename Po
intT>
void
69 if (input_cloud_ != NULL)
70 input_cloud_.reset ();
72 input_cloud_ = input_cloud;
75 std::vector<pcl::PCLPointField> fields;
80 if (label_index != -1)
85 template <
typename Po
intT>
void
91 out->
width =
static_cast<int> (out->
points.size ());
95 for (
size_t i = 0; i < in->
points.size (); i++)
100 point.y = in->
points[i].y;
101 point.z = in->
points[i].z;
106 template <
typename Po
intT>
void
114 out->
width =
static_cast<int> (out->
points.size ());
118 for (
size_t i = 0; i < in->
points.size (); i++)
122 point.x = in->
points[i].x;
123 point.y = in->
points[i].y;
124 point.z = in->
points[i].z;
133 template <
typename Po
intT>
void
135 std::vector<int> &cluster_numbers)
138 std::vector <pcl::PCLPointField> fields;
145 for (
size_t i = 0; i < in->
points.size (); i++)
149 memcpy (&label,
reinterpret_cast<char*
> (&in->
points[i]) + fields[label_idx].offset,
sizeof(uint32_t));
153 for (
size_t j = 0; j < cluster_numbers.size (); j++)
155 if (
static_cast<uint32_t
> (cluster_numbers[j]) == label)
162 cluster_numbers.push_back (label);
168 template <
typename Po
intT>
void
174 std::vector <pcl::PCLPointField> fields;
181 for (
size_t i = 0; i < in->
points.size (); i++)
185 memcpy (&label,
reinterpret_cast<char*
> (&in->
points[i]) + fields[label_idx].offset,
sizeof(uint32_t));
187 if (
static_cast<int> (label) == label_num)
191 point.x = in->
points[i].x;
192 point.y = in->
points[i].y;
193 point.z = in->
points[i].z;
194 out->
points.push_back (point);
197 out->
width =
static_cast<int> (out->
points.size ());
204 template <
typename Po
intT>
void
207 float normal_radius_search,
208 float fpfh_radius_search)
233 template <
typename Po
intT>
void
242 for (
size_t i = 0; i < in->
points.size (); i++)
244 std::vector<float> data (33);
245 for (
int idx = 0; idx < 33; idx++)
246 data[idx] = in->
points[i].histogram[idx];
257 out->
width =
static_cast<int> (centroids.size ());
260 out->
points.resize (
static_cast<int> (centroids.size ()));
262 for (
size_t i = 0; i < centroids.size (); i++)
265 for (
int idx = 0; idx < 33; idx++)
266 point.
histogram[idx] = centroids[i][idx];
272 template <
typename Po
intT>
void
275 std::vector<int> &indi,
276 std::vector<float> &dist)
280 for (
size_t i = 0; i < trained_features.size (); i++)
281 n_row +=
static_cast<int> (trained_features[i]->points.size ());
286 for (
size_t k = 0; k < trained_features.size (); k++)
289 size_t c = hist->
points.size ();
290 for (
size_t i = 0; i < c; ++i)
291 for (
size_t j = 0; j < data.cols; ++j)
292 data[(k * c) + i][j] = hist->
points[i].histogram[j];
301 index->buildIndex ();
304 indi.resize (query_features->
points.size ());
305 dist.resize (query_features->
points.size ());
307 for (
size_t i = 0; i < query_features->
points.size (); i++)
311 memcpy (&p.ptr ()[0], query_features->
points[i].histogram, p.cols * p.rows * sizeof (
float));
315 index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
317 indi[i] = indices[0][0];
318 dist[i] = distances[0][0];
325 delete[] data.ptr ();
329 template <
typename Po
intT>
void
331 std::vector<float> &dist,
333 float feature_threshold,
337 float nfm =
static_cast<float> (n_feature_means);
338 for (
size_t i = 0; i < out->
points.size (); i++)
340 if (dist[i] < feature_threshold)
342 float l =
static_cast<float> (indi[i]) / nfm;
345 std::modf (l , &intpart);
346 int label =
static_cast<int> (intpart);
348 out->
points[i].label = label+2;
355 template <
typename Po
intT>
void
360 convertCloud (input_cloud_, tmp_cloud);
364 computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
369 kmeansClustering (feature, output, cluster_size_);
373 template <
typename Po
intT>
void
378 std::vector<int> cluster_numbers;
379 findClusters (input_cloud_, cluster_numbers);
380 std::cout <<
"cluster numbers: ";
381 for (
size_t i = 0; i < cluster_numbers.size (); i++)
382 std::cout << cluster_numbers[i] <<
" ";
383 std::cout << std::endl;
385 for (
size_t i = 0; i < cluster_numbers.size (); i++)
389 getCloudWithLabel (input_cloud_, label_cloud, cluster_numbers[i]);
393 computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
397 kmeansClustering (feature, kmeans_feature, cluster_size_);
399 output.push_back (*kmeans_feature);
404 template <
typename Po
intT>
void
407 if (trained_features_.size () > 0)
411 convertCloud (input_cloud_, tmp_cloud);
415 computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
418 std::vector<int> indices;
420 queryFeatureDistances (trained_features_, input_cloud_features, indices,
distance);
423 int n_feature_means =
static_cast<int> (trained_features_[0]->points.size ());
424 convertCloud (input_cloud_, out);
425 assignLabels (indices,
distance, n_feature_means, feature_threshold_, out);
429 PCL_ERROR (
"no training features set \n");
433 #define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier<T>;
435 #endif // PCL_UNARY_CLASSIFIER_HPP_