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)
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_);
419 std::vector<int> indices;
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>;
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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, std::vector< int > &indi, std::vector< float > &dist)
void assignLabels(std::vector< int > &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
float distance(const PointT &p1, const PointT &p2)
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.