40 #ifndef PCL_UNARY_CLASSIFIER_HPP_
41 #define PCL_UNARY_CLASSIFIER_HPP_
44 #include <flann/algorithms/center_chooser.h>
45 #include <flann/util/matrix.h>
47 #include <pcl/segmentation/unary_classifier.h>
48 #include <pcl/common/io.h>
51 template <
typename Po
intT>
55 normal_radius_search_ (0.01f),
56 fpfh_radius_search_ (0.05f),
57 feature_threshold_ (5.0)
62 template <
typename Po
intT>
68 template <
typename Po
intT>
void
71 input_cloud_ = input_cloud;
74 std::vector<pcl::PCLPointField> fields;
77 label_index = pcl::getFieldIndex<PointT> (
"label", fields);
79 if (label_index != -1)
84 template <
typename Po
intT>
void
90 out->
width = static_cast<int> (out->
points.size ());
94 for (std::size_t i = 0; i < in->
points.size (); i++)
100 point.z = in->
points[i].z;
105 template <
typename Po
intT>
void
113 out->
width = static_cast<int> (out->
points.size ());
117 for (std::size_t i = 0; i < in->
points.size (); i++)
121 point.x = in->
points[i].x;
122 point.y = in->
points[i].y;
123 point.z = in->
points[i].z;
132 template <
typename Po
intT>
void
134 std::vector<int> &cluster_numbers)
137 std::vector <pcl::PCLPointField> fields;
140 label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
144 for (std::size_t i = 0; i < in->
points.size (); i++)
148 memcpy (&label, reinterpret_cast<char*> (&in->
points[i]) + fields[label_idx].offset,
sizeof(std::uint32_t));
152 for (
const int &cluster_number : cluster_numbers)
154 if (static_cast<std::uint32_t> (cluster_number) == label)
161 cluster_numbers.push_back (label);
167 template <
typename Po
intT>
void
173 std::vector <pcl::PCLPointField> fields;
176 label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
180 for (std::size_t i = 0; i < in->
points.size (); i++)
184 memcpy (&label, reinterpret_cast<char*> (&in->
points[i]) + fields[label_idx].offset,
sizeof(std::uint32_t));
186 if (static_cast<int> (label) == label_num)
190 point.x = in->
points[i].x;
191 point.y = in->
points[i].y;
192 point.z = in->
points[i].z;
193 out->
points.push_back (point);
196 out->
width = static_cast<int> (out->
points.size ());
203 template <
typename Po
intT>
void
206 float normal_radius_search,
207 float fpfh_radius_search)
232 template <
typename Po
intT>
void
241 for (
const auto &point : in->
points)
243 std::vector<float> data (33);
244 for (
int idx = 0; idx < 33; idx++)
245 data[idx] = point.histogram[idx];
256 out->
width = static_cast<int> (centroids.size ());
259 out->
points.resize (static_cast<int> (centroids.size ()));
261 for (std::size_t i = 0; i < centroids.size (); i++)
264 for (
int idx = 0; idx < 33; idx++)
265 point.
histogram[idx] = centroids[i][idx];
271 template <
typename Po
intT>
void
274 std::vector<int> &indi,
275 std::vector<float> &dist)
279 for (
const auto &trained_feature : trained_features)
280 n_row += static_cast<int> (trained_feature->points.size ());
285 for (std::size_t k = 0; k < trained_features.size (); k++)
288 std::size_t c = hist->
points.size ();
289 for (std::size_t i = 0; i < c; ++i)
290 for (std::size_t j = 0; j < data.cols; ++j)
291 data[(k * c) + i][j] = hist->
points[i].histogram[j];
300 index->buildIndex ();
303 indi.resize (query_features->
points.size ());
304 dist.resize (query_features->
points.size ());
306 for (std::size_t i = 0; i < query_features->
points.size (); i++)
310 memcpy (&p.ptr ()[0], query_features->
points[i].histogram, p.cols * p.rows *
sizeof (float));
314 index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
316 indi[i] = indices[0][0];
317 dist[i] = distances[0][0];
324 delete[] data.ptr ();
328 template <
typename Po
intT>
void
330 std::vector<float> &dist,
332 float feature_threshold,
336 float nfm = static_cast<float> (n_feature_means);
337 for (std::size_t i = 0; i < out->
points.size (); i++)
339 if (dist[i] < feature_threshold)
341 float l = static_cast<float> (indi[i]) / nfm;
344 std::modf (l , &intpart);
345 int label = static_cast<int> (intpart);
347 out->
points[i].label = label+2;
354 template <
typename Po
intT>
void
359 convertCloud (input_cloud_, tmp_cloud);
363 computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
368 kmeansClustering (feature, output, cluster_size_);
372 template <
typename Po
intT>
void
377 std::vector<int> cluster_numbers;
378 findClusters (input_cloud_, cluster_numbers);
379 std::cout <<
"cluster numbers: ";
380 for (
const int &cluster_number : cluster_numbers)
381 std::cout << cluster_number <<
" ";
382 std::cout << std::endl;
384 for (
const int &cluster_number : cluster_numbers)
388 getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
392 computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
396 kmeansClustering (feature, kmeans_feature, cluster_size_);
398 output.push_back (*kmeans_feature);
403 template <
typename Po
intT>
void
406 if (!trained_features_.empty ())
410 convertCloud (input_cloud_, tmp_cloud);
414 computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
417 std::vector<int> indices;
419 queryFeatureDistances (trained_features_, input_cloud_features, indices,
distance);
422 int n_feature_means = static_cast<int> (trained_features_[0]->points.size ());
423 convertCloud (input_cloud_, out);
424 assignLabels (indices,
distance, n_feature_means, feature_threshold_, out);
428 PCL_ERROR (
"no training features set \n");
432 #define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier<T>;
434 #endif // PCL_UNARY_CLASSIFIER_HPP_