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>
53template <
typename Po
intT>
57 normal_radius_search_ (0.01f),
58 fpfh_radius_search_ (0.05f),
59 feature_threshold_ (5.0)
64template <
typename Po
intT>
70template <
typename Po
intT>
void
73 input_cloud_ = input_cloud;
75 pcl::PointCloud <PointT> point;
76 std::vector<pcl::PCLPointField> fields;
81 if (label_index != -1)
86template <
typename Po
intT>
void
91 out->points.resize (in->size ());
92 out->width = out->size ();
94 out->is_dense =
false;
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;
107template <
typename Po
intT>
void
114 out->points.resize (in->size ());
115 out->width = out->size ();
117 out->is_dense =
false;
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;
134template <
typename Po
intT>
void
136 std::vector<int> &cluster_numbers)
139 std::vector <pcl::PCLPointField> fields;
141 pcl::PointCloud <PointT> point;
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);
169template <
typename Po
intT>
void
175 std::vector <pcl::PCLPointField> fields;
181 for (
const auto& point : (*in))
185 memcpy (&label,
reinterpret_cast<const char*
> (&point) + fields[label_idx].offset,
sizeof(std::uint32_t));
187 if (
static_cast<int> (label) == label_num)
194 out->push_back (tmp);
197 out->width = out->size ();
199 out->is_dense =
false;
204template <
typename Po
intT>
void
207 float normal_radius_search,
208 float fpfh_radius_search)
233template <
typename Po
intT>
void
238 pcl::Kmeans kmeans (
static_cast<int> (in->size ()), 33);
242 for (
const auto &point : in->points)
244 std::vector<float> data (33);
245 for (
int idx = 0; idx < 33; idx++)
246 data[idx] = point.histogram[idx];
257 out->width = centroids.size ();
259 out->is_dense =
false;
260 out->points.resize (
static_cast<int> (centroids.size ()));
262 for (std::size_t i = 0; i < centroids.size (); i++)
265 for (
int idx = 0; idx < 33; idx++)
266 point.
histogram[idx] = centroids[i][idx];
272template <
typename Po
intT>
void
276 std::vector<float> &dist)
280 for (
const auto &trained_feature : trained_features)
281 n_row +=
static_cast<int> (trained_feature->size ());
286 for (std::size_t k = 0; k < trained_features.size (); 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];
301 index->buildIndex ();
304 indi.resize (query_features->size ());
305 dist.resize (query_features->size ());
307 for (std::size_t i = 0; i < query_features->size (); i++)
311 memcpy (&p.ptr ()[0], (*query_features)[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 ();
329template <
typename Po
intT>
void
331 std::vector<float> &dist,
333 float feature_threshold,
337 float nfm =
static_cast<float> (n_feature_means);
338 for (std::size_t i = 0; i < out->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)[i].label = label+2;
355template <
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_);
373template <
typename Po
intT>
void
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;
385 for (
const int &cluster_number : cluster_numbers)
389 getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
393 computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
397 kmeansClustering (feature, kmeans_feature, cluster_size_);
399 output.push_back (*kmeans_feature);
404template <
typename Po
intT>
void
407 if (!trained_features_.empty ())
411 convertCloud (input_cloud_, tmp_cloud);
415 computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
419 std::vector<float> distance;
420 queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
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);
429 PCL_ERROR (
"no training features set \n");
433#define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier<T>;
FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud d...
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Centroids get_centroids()
void addDataPoint(Point &data_point)
void setClusterSize(unsigned int k)
This method sets the k-means cluster size.
std::vector< Point > Centroids
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
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...
shared_ptr< KdTree< PointT, Tree > > Ptr
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
IndicesAllocator<> Indices
Type used for indices in PCL.
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.