40 #ifndef PCL_UNARY_CLASSIFIER_HPP_
41 #define PCL_UNARY_CLASSIFIER_HPP_
44 #include <flann/flann.hpp>
45 #include <flann/algorithms/dist.h>
46 #include <flann/algorithms/linear_index.h>
47 #include <flann/util/matrix.h>
49 #include <pcl/segmentation/unary_classifier.h>
50 #include <pcl/common/io.h>
53 template <
typename Po
intT>
57 normal_radius_search_ (0.01f),
58 fpfh_radius_search_ (0.05f),
59 feature_threshold_ (5.0)
64 template <
typename Po
intT>
70 template <
typename Po
intT>
void
73 input_cloud_ = input_cloud;
76 std::vector<pcl::PCLPointField> fields;
79 label_index = pcl::getFieldIndex<PointT> (
"label", fields);
81 if (label_index != -1)
86 template <
typename Po
intT>
void
96 for (std::size_t i = 0; i < in->
size (); i++)
100 point.x = (*in)[i].x;
101 point.y = (*in)[i].y;
102 point.z = (*in)[i].z;
107 template <
typename Po
intT>
void
119 for (std::size_t i = 0; i < in->
size (); i++)
123 point.x = (*in)[i].x;
124 point.y = (*in)[i].y;
125 point.z = (*in)[i].z;
134 template <
typename Po
intT>
void
136 std::vector<int> &cluster_numbers)
139 std::vector <pcl::PCLPointField> fields;
142 label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
146 for (
const auto& point: *in)
150 memcpy (&label,
reinterpret_cast<const char*
> (&point) + fields[label_idx].offset,
sizeof(std::uint32_t));
154 for (
const int &cluster_number : cluster_numbers)
156 if (
static_cast<std::uint32_t
> (cluster_number) == label)
163 cluster_numbers.push_back (label);
169 template <
typename Po
intT>
void
175 std::vector <pcl::PCLPointField> fields;
178 label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
182 for (std::size_t i = 0; i < in->
size (); i++)
186 memcpy (&label,
reinterpret_cast<char*
> (&(*in)[i]) + fields[label_idx].offset,
sizeof(std::uint32_t));
188 if (
static_cast<int> (label) == label_num)
192 point.x = (*in)[i].x;
193 point.y = (*in)[i].y;
194 point.z = (*in)[i].z;
195 out->
points.push_back (point);
205 template <
typename Po
intT>
void
208 float normal_radius_search,
209 float fpfh_radius_search)
234 template <
typename Po
intT>
void
243 for (
const auto &point : in->
points)
245 std::vector<float> data (33);
246 for (
int idx = 0; idx < 33; idx++)
247 data[idx] = point.histogram[idx];
258 out->
width = centroids.size ();
261 out->
points.resize (
static_cast<int> (centroids.size ()));
263 for (std::size_t i = 0; i < centroids.size (); i++)
266 for (
int idx = 0; idx < 33; idx++)
267 point.
histogram[idx] = centroids[i][idx];
273 template <
typename Po
intT>
void
276 std::vector<int> &indi,
277 std::vector<float> &dist)
281 for (
const auto &trained_feature : trained_features)
282 n_row +=
static_cast<int> (trained_feature->size ());
287 for (std::size_t k = 0; k < trained_features.size (); k++)
290 const auto c = hist->
size ();
291 for (std::size_t i = 0; i < c; ++i)
292 for (std::size_t j = 0; j < data.cols; ++j)
293 data[(k * c) + i][j] = (*hist)[i].histogram[j];
302 index->buildIndex ();
305 indi.resize (query_features->
size ());
306 dist.resize (query_features->
size ());
308 for (std::size_t i = 0; i < query_features->
size (); i++)
312 memcpy (&p.ptr ()[0], (*query_features)[i].histogram, p.cols * p.rows * sizeof (
float));
316 index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
318 indi[i] = indices[0][0];
319 dist[i] = distances[0][0];
326 delete[] data.ptr ();
330 template <
typename Po
intT>
void
332 std::vector<float> &dist,
334 float feature_threshold,
338 float nfm =
static_cast<float> (n_feature_means);
339 for (std::size_t i = 0; i < out->
size (); i++)
341 if (dist[i] < feature_threshold)
343 float l =
static_cast<float> (indi[i]) / nfm;
346 std::modf (l , &intpart);
347 int label =
static_cast<int> (intpart);
349 (*out)[i].label = label+2;
356 template <
typename Po
intT>
void
361 convertCloud (input_cloud_, tmp_cloud);
365 computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
370 kmeansClustering (feature, output, cluster_size_);
374 template <
typename Po
intT>
void
379 std::vector<int> cluster_numbers;
380 findClusters (input_cloud_, cluster_numbers);
381 std::cout <<
"cluster numbers: ";
382 for (
const int &cluster_number : cluster_numbers)
383 std::cout << cluster_number <<
" ";
384 std::cout << std::endl;
386 for (
const int &cluster_number : cluster_numbers)
390 getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
394 computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
398 kmeansClustering (feature, kmeans_feature, cluster_size_);
400 output.push_back (*kmeans_feature);
405 template <
typename Po
intT>
void
408 if (!trained_features_.empty ())
412 convertCloud (input_cloud_, tmp_cloud);
416 computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
421 queryFeatureDistances (trained_features_, input_cloud_features, indices,
distance);
424 const auto n_feature_means = trained_features_[0]->size ();
425 convertCloud (input_cloud_, out);
426 assignLabels (indices,
distance, n_feature_means, feature_threshold_, out);
430 PCL_ERROR (
"no training features set \n");
434 #define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier<T>;
436 #endif // PCL_UNARY_CLASSIFIER_HPP_