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/features/normal_3d.h>
50 #include <pcl/segmentation/unary_classifier.h>
51 #include <pcl/common/io.h>
54 template <
typename Po
intT>
58 template <
typename Po
intT>
62 template <
typename Po
intT>
void
65 input_cloud_ = input_cloud;
68 std::vector<pcl::PCLPointField> fields;
71 label_index = pcl::getFieldIndex<PointT> (
"label", fields);
73 if (label_index != -1)
78 template <
typename Po
intT>
void
88 for (std::size_t i = 0; i < in->
size (); i++)
99 template <
typename Po
intT>
void
111 for (std::size_t i = 0; i < in->
size (); i++)
115 point.x = (*in)[i].x;
116 point.y = (*in)[i].y;
117 point.z = (*in)[i].z;
126 template <
typename Po
intT>
void
128 std::vector<int> &cluster_numbers)
131 std::vector <pcl::PCLPointField> fields;
132 const int label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
136 for (
const auto& point: *in)
140 memcpy (&label, reinterpret_cast<const char*> (&point) + fields[label_idx].offset,
sizeof(std::uint32_t));
144 for (
const int &cluster_number : cluster_numbers)
146 if (static_cast<std::uint32_t> (cluster_number) == label)
153 cluster_numbers.push_back (label);
159 template <
typename Po
intT>
void
165 std::vector <pcl::PCLPointField> fields;
167 label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
171 for (
const auto& point : (*in))
175 memcpy (&label, reinterpret_cast<const char*> (&point) + fields[label_idx].offset,
sizeof(std::uint32_t));
177 if (static_cast<int> (label) == label_num)
194 template <
typename Po
intT>
void
197 float normal_radius_search,
198 float fpfh_radius_search)
223 template <
typename Po
intT>
void
232 for (
const auto &point : in->
points)
234 std::vector<float> data (33);
235 for (
int idx = 0; idx < 33; idx++)
236 data[idx] = point.histogram[idx];
247 out->
width = centroids.size ();
250 out->
points.resize (static_cast<int> (centroids.size ()));
252 for (std::size_t i = 0; i < centroids.size (); i++)
255 for (
int idx = 0; idx < 33; idx++)
256 point.
histogram[idx] = centroids[i][idx];
262 template <
typename Po
intT>
void
266 std::vector<float> &dist)
270 for (
const auto &trained_feature : trained_features)
271 n_row +=
static_cast<int> (trained_feature->size ());
276 for (std::size_t k = 0; k < trained_features.size (); 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];
291 index->buildIndex ();
294 indi.resize (query_features->
size ());
295 dist.resize (query_features->
size ());
297 for (std::size_t i = 0; i < query_features->
size (); i++)
301 std::copy((*query_features)[i].histogram, (*query_features)[i].histogram + n_col, p.ptr());
305 index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
307 indi[i] = indices[0][0];
308 dist[i] = distances[0][0];
315 delete[] data.ptr ();
319 template <
typename Po
intT>
void
321 std::vector<float> &dist,
323 float feature_threshold,
327 float nfm =
static_cast<float> (n_feature_means);
328 for (std::size_t i = 0; i < out->
size (); i++)
330 if (dist[i] < feature_threshold)
332 float l =
static_cast<float> (indi[i]) / nfm;
335 std::modf (l , &intpart);
336 int label =
static_cast<int> (intpart);
338 (*out)[i].label = label+2;
345 template <
typename Po
intT>
void
350 convertCloud (input_cloud_, tmp_cloud);
354 computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
359 kmeansClustering (feature, output, cluster_size_);
363 template <
typename Po
intT>
void
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;
375 for (
const int &cluster_number : cluster_numbers)
379 getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
383 computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
387 kmeansClustering (feature, kmeans_feature, cluster_size_);
389 output.push_back (*kmeans_feature);
394 template <
typename Po
intT>
void
397 if (!trained_features_.empty ())
401 convertCloud (input_cloud_, tmp_cloud);
405 computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
409 std::vector<float> distance;
410 queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
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);
419 PCL_ERROR (
"no training features set \n");
423 #define PCL_INSTANTIATE_UnaryClassifier(T) template class PCL_EXPORTS pcl::UnaryClassifier<T>;
425 #endif // PCL_UNARY_CLASSIFIER_HPP_
shared_ptr< KdTree< PointT, Tree > > Ptr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
void addDataPoint(Point &data_point)
shared_ptr< PointCloud< PointT > > Ptr
PCL_ADD_POINT4D PCL_ADD_RGB std::uint32_t label
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
std::vector< Point > Centroids
void convertCloud(typename pcl::PointCloud< PointT >::Ptr in, pcl::PointCloud< pcl::PointXYZ >::Ptr out)
void train(pcl::PointCloud< pcl::FPFHSignature33 >::Ptr &output)
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
void findClusters(typename pcl::PointCloud< PointT >::Ptr in, std::vector< int > &cluster_numbers)
A point structure representing the Fast Point Feature Histogram (FPFH).
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)
std::uint32_t width
The point cloud width (if organized as an image-structure).
void setClusterSize(unsigned int k)
This method sets the k-means cluster size.
void kmeansClustering(pcl::PointCloud< pcl::FPFHSignature33 >::Ptr in, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr out, int k)
Centroids get_centroids()
A point structure representing Euclidean xyz coordinates.
~UnaryClassifier()
This destructor destroys the cloud...
void setInputCloud(typename pcl::PointCloud< PointT >::Ptr input_cloud)
This method sets the input cloud.
FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud d...
std::uint32_t height
The point cloud height (if organized as an image-structure).
IndicesAllocator<> Indices
Type used for indices in PCL.
void trainWithLabel(std::vector< pcl::PointCloud< pcl::FPFHSignature33 >, Eigen::aligned_allocator< pcl::PointCloud< pcl::FPFHSignature33 > > > &output)
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
void assignLabels(pcl::Indices &indi, std::vector< float > &dist, int n_feature_means, float feature_threshold, pcl::PointCloud< pcl::PointXYZRGBL >::Ptr out)
void segment(pcl::PointCloud< pcl::PointXYZRGBL >::Ptr &out)
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
void computeFPFH(pcl::PointCloud< pcl::PointXYZ >::Ptr in, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr out, float normal_radius_search, float fpfh_radius_search)
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
UnaryClassifier()
Constructor that sets default values for member variables.
void getCloudWithLabel(typename pcl::PointCloud< PointT >::Ptr in, pcl::PointCloud< pcl::PointXYZ >::Ptr out, int label_num)