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 normal_radius_search_ (0.01f),
59 fpfh_radius_search_ (0.05f),
60 feature_threshold_ (5.0)
65 template <
typename Po
intT>
69 template <
typename Po
intT>
void
72 input_cloud_ = input_cloud;
75 std::vector<pcl::PCLPointField> fields;
78 label_index = pcl::getFieldIndex<PointT> (
"label", fields);
80 if (label_index != -1)
85 template <
typename Po
intT>
void
95 for (std::size_t i = 0; i < in->
size (); i++)
100 point.y = (*in)[i].y;
101 point.z = (*in)[i].z;
106 template <
typename Po
intT>
void
118 for (std::size_t i = 0; i < in->
size (); i++)
122 point.x = (*in)[i].x;
123 point.y = (*in)[i].y;
124 point.z = (*in)[i].z;
133 template <
typename Po
intT>
void
135 std::vector<int> &cluster_numbers)
138 std::vector <pcl::PCLPointField> fields;
139 const int label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
143 for (
const auto& point: *in)
147 memcpy (&label,
reinterpret_cast<const char*
> (&point) + fields[label_idx].offset,
sizeof(std::uint32_t));
151 for (
const int &cluster_number : cluster_numbers)
153 if (
static_cast<std::uint32_t
> (cluster_number) == label)
160 cluster_numbers.push_back (label);
166 template <
typename Po
intT>
void
172 std::vector <pcl::PCLPointField> fields;
174 label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
178 for (
const auto& point : (*in))
182 memcpy (&label,
reinterpret_cast<const char*
> (&point) + fields[label_idx].offset,
sizeof(std::uint32_t));
184 if (
static_cast<int> (label) == label_num)
201 template <
typename Po
intT>
void
204 float normal_radius_search,
205 float fpfh_radius_search)
230 template <
typename Po
intT>
void
239 for (
const auto &point : in->
points)
241 std::vector<float> data (33);
242 for (
int idx = 0; idx < 33; idx++)
243 data[idx] = point.histogram[idx];
254 out->
width = centroids.size ();
257 out->
points.resize (
static_cast<int> (centroids.size ()));
259 for (std::size_t i = 0; i < centroids.size (); i++)
262 for (
int idx = 0; idx < 33; idx++)
263 point.
histogram[idx] = centroids[i][idx];
269 template <
typename Po
intT>
void
273 std::vector<float> &dist)
277 for (
const auto &trained_feature : trained_features)
278 n_row +=
static_cast<int> (trained_feature->size ());
283 for (std::size_t k = 0; k < trained_features.size (); k++)
286 const auto c = hist->
size ();
287 for (std::size_t i = 0; i < c; ++i)
288 for (std::size_t j = 0; j < data.cols; ++j)
289 data[(k * c) + i][j] = (*hist)[i].histogram[j];
298 index->buildIndex ();
301 indi.resize (query_features->
size ());
302 dist.resize (query_features->
size ());
304 for (std::size_t i = 0; i < query_features->
size (); i++)
308 std::copy((*query_features)[i].histogram, (*query_features)[i].histogram + n_col, p.ptr());
312 index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
314 indi[i] = indices[0][0];
315 dist[i] = distances[0][0];
322 delete[] data.ptr ();
326 template <
typename Po
intT>
void
328 std::vector<float> &dist,
330 float feature_threshold,
334 float nfm =
static_cast<float> (n_feature_means);
335 for (std::size_t i = 0; i < out->
size (); i++)
337 if (dist[i] < feature_threshold)
339 float l =
static_cast<float> (indi[i]) / nfm;
342 std::modf (l , &intpart);
343 int label =
static_cast<int> (intpart);
345 (*out)[i].label = label+2;
352 template <
typename Po
intT>
void
357 convertCloud (input_cloud_, tmp_cloud);
361 computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
366 kmeansClustering (feature, output, cluster_size_);
370 template <
typename Po
intT>
void
375 std::vector<int> cluster_numbers;
376 findClusters (input_cloud_, cluster_numbers);
377 std::cout <<
"cluster numbers: ";
378 for (
const int &cluster_number : cluster_numbers)
379 std::cout << cluster_number <<
" ";
380 std::cout << std::endl;
382 for (
const int &cluster_number : cluster_numbers)
386 getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
390 computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
394 kmeansClustering (feature, kmeans_feature, cluster_size_);
396 output.push_back (*kmeans_feature);
401 template <
typename Po
intT>
void
404 if (!trained_features_.empty ())
408 convertCloud (input_cloud_, tmp_cloud);
412 computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
417 queryFeatureDistances (trained_features_, input_cloud_features, indices,
distance);
420 const auto n_feature_means = trained_features_[0]->size ();
421 convertCloud (input_cloud_, out);
422 assignLabels (indices,
distance, n_feature_means, feature_threshold_, out);
426 PCL_ERROR (
"no training features set \n");
430 #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.