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;
177 label_idx = pcl::getFieldIndex<PointT> (
"label", 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)
204 template <
typename Po
intT>
void
207 float normal_radius_search,
208 float fpfh_radius_search)
233 template <
typename Po
intT>
void
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 ();
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];
272 template <
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 ();
329 template <
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;
355 template <
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_);
373 template <
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);
404 template <
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_);
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.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
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, 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
float distance(const PointT &p1, const PointT &p2)
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.