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>
52 #include <pcl/search/kdtree.h>
55 template <
typename Po
intT>
59 template <
typename Po
intT>
63 template <
typename Po
intT>
void
66 input_cloud_ = input_cloud;
69 std::vector<pcl::PCLPointField> fields;
72 label_index = pcl::getFieldIndex<PointT> (
"label", fields);
74 if (label_index != -1)
79 template <
typename Po
intT>
void
89 for (std::size_t i = 0; i < in->
size (); i++)
100 template <
typename Po
intT>
void
112 for (std::size_t i = 0; i < in->
size (); i++)
116 point.x = (*in)[i].x;
117 point.y = (*in)[i].y;
118 point.z = (*in)[i].z;
127 template <
typename Po
intT>
void
129 std::vector<int> &cluster_numbers)
132 std::vector <pcl::PCLPointField> fields;
133 const int label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
137 for (
const auto& point: *in)
141 memcpy (&label,
reinterpret_cast<const char*
> (&point) + fields[label_idx].offset,
sizeof(std::uint32_t));
145 for (
const int &cluster_number : cluster_numbers)
147 if (
static_cast<std::uint32_t
> (cluster_number) == label)
154 cluster_numbers.push_back (label);
160 template <
typename Po
intT>
void
166 std::vector <pcl::PCLPointField> fields;
168 label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
172 for (
const auto& point : (*in))
176 memcpy (&label,
reinterpret_cast<const char*
> (&point) + fields[label_idx].offset,
sizeof(std::uint32_t));
178 if (
static_cast<int> (label) == label_num)
195 template <
typename Po
intT>
void
198 float normal_radius_search,
199 float fpfh_radius_search)
224 template <
typename Po
intT>
void
233 for (
const auto &point : in->
points)
235 std::vector<float> data (33);
236 for (
int idx = 0; idx < 33; idx++)
237 data[idx] = point.histogram[idx];
248 out->
width = centroids.size ();
251 out->
points.resize (
static_cast<int> (centroids.size ()));
253 for (std::size_t i = 0; i < centroids.size (); i++)
256 for (
int idx = 0; idx < 33; idx++)
257 point.
histogram[idx] = centroids[i][idx];
263 template <
typename Po
intT>
void
267 std::vector<float> &dist)
271 for (
const auto &trained_feature : trained_features)
272 n_row +=
static_cast<int> (trained_feature->size ());
277 for (std::size_t k = 0; k < trained_features.size (); k++)
280 const auto c = hist->
size ();
281 for (std::size_t i = 0; i < c; ++i)
282 for (std::size_t j = 0; j < data.cols; ++j)
283 data[(k * c) + i][j] = (*hist)[i].histogram[j];
292 index->buildIndex ();
295 indi.resize (query_features->
size ());
296 dist.resize (query_features->
size ());
298 for (std::size_t i = 0; i < query_features->
size (); i++)
302 std::copy((*query_features)[i].histogram, (*query_features)[i].histogram + n_col, p.ptr());
306 index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
308 indi[i] = indices[0][0];
309 dist[i] = distances[0][0];
316 delete[] data.ptr ();
320 template <
typename Po
intT>
void
322 std::vector<float> &dist,
324 float feature_threshold,
328 float nfm =
static_cast<float> (n_feature_means);
329 for (std::size_t i = 0; i < out->
size (); i++)
331 if (dist[i] < feature_threshold)
333 float l =
static_cast<float> (indi[i]) / nfm;
336 std::modf (l , &intpart);
337 int label =
static_cast<int> (intpart);
339 (*out)[i].label = label+2;
346 template <
typename Po
intT>
void
351 convertCloud (input_cloud_, tmp_cloud);
355 computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
360 kmeansClustering (feature, output, cluster_size_);
364 template <
typename Po
intT>
void
369 std::vector<int> cluster_numbers;
370 findClusters (input_cloud_, cluster_numbers);
371 std::cout <<
"cluster numbers: ";
372 for (
const int &cluster_number : cluster_numbers)
373 std::cout << cluster_number <<
" ";
374 std::cout << std::endl;
376 for (
const int &cluster_number : cluster_numbers)
380 getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
384 computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
388 kmeansClustering (feature, kmeans_feature, cluster_size_);
390 output.push_back (*kmeans_feature);
395 template <
typename Po
intT>
void
398 if (!trained_features_.empty ())
402 convertCloud (input_cloud_, tmp_cloud);
406 computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
411 queryFeatureDistances (trained_features_, input_cloud_features, indices,
distance);
414 const auto n_feature_means = trained_features_[0]->size ();
415 convertCloud (input_cloud_, out);
416 assignLabels (indices,
distance, n_feature_means, feature_threshold_, out);
420 PCL_ERROR (
"no training features set \n");
424 #define PCL_INSTANTIATE_UnaryClassifier(T) template class PCL_EXPORTS 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.
PCL_ADD_POINT4D PCL_ADD_RGB std::uint32_t label
A point structure representing the Fast Point Feature Histogram (FPFH).
A point structure representing Euclidean xyz coordinates.