40 #ifndef PCL_UNARY_CLASSIFIER_HPP_
41 #define PCL_UNARY_CLASSIFIER_HPP_
44 #include <flann/algorithms/center_chooser.h>
45 #include <flann/util/matrix.h>
47 #include <pcl/segmentation/unary_classifier.h>
48 #include <pcl/common/io.h>
51 template <
typename Po
intT>
55 normal_radius_search_ (0.01f),
56 fpfh_radius_search_ (0.05f),
57 feature_threshold_ (5.0)
62 template <
typename Po
intT>
68 template <
typename Po
intT>
void
71 input_cloud_ = input_cloud;
74 std::vector<pcl::PCLPointField> fields;
77 label_index = pcl::getFieldIndex<PointT> (
"label", fields);
79 if (label_index != -1)
84 template <
typename Po
intT>
void
90 out->
width =
static_cast<int> (out->
points.size ());
94 for (std::size_t i = 0; i < in->
points.size (); i++)
100 point.z = in->
points[i].z;
105 template <
typename Po
intT>
void
113 out->
width =
static_cast<int> (out->
points.size ());
117 for (std::size_t i = 0; i < in->
points.size (); i++)
121 point.x = in->
points[i].x;
122 point.y = in->
points[i].y;
123 point.z = in->
points[i].z;
132 template <
typename Po
intT>
void
134 std::vector<int> &cluster_numbers)
137 std::vector <pcl::PCLPointField> fields;
140 label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
144 for (std::size_t i = 0; i < in->
points.size (); i++)
148 memcpy (&label, reinterpret_cast<char*> (&in->
points[i]) + fields[label_idx].offset,
sizeof(std::uint32_t));
152 for (
const int &cluster_number : cluster_numbers)
154 if (static_cast<std::uint32_t> (cluster_number) == label)
161 cluster_numbers.push_back (label);
167 template <
typename Po
intT>
void
173 std::vector <pcl::PCLPointField> fields;
176 label_idx = pcl::getFieldIndex<PointT> (
"label", fields);
180 for (std::size_t i = 0; i < in->
points.size (); i++)
184 memcpy (&label, reinterpret_cast<char*> (&in->
points[i]) + fields[label_idx].offset,
sizeof(std::uint32_t));
186 if (static_cast<int> (label) == label_num)
190 point.x = in->
points[i].x;
191 point.y = in->
points[i].y;
192 point.z = in->
points[i].z;
193 out->
points.push_back (point);
196 out->
width =
static_cast<int> (out->
points.size ());
203 template <
typename Po
intT>
void
206 float normal_radius_search,
207 float fpfh_radius_search)
232 template <
typename Po
intT>
void
241 for (
const auto &point : in->
points)
243 std::vector<float> data (33);
244 for (
int idx = 0; idx < 33; idx++)
245 data[idx] = point.histogram[idx];
256 out->
width =
static_cast<int> (centroids.size ());
259 out->
points.resize (static_cast<int> (centroids.size ()));
261 for (std::size_t i = 0; i < centroids.size (); i++)
264 for (
int idx = 0; idx < 33; idx++)
265 point.
histogram[idx] = centroids[i][idx];
271 template <
typename Po
intT>
void
274 std::vector<int> &indi,
275 std::vector<float> &dist)
279 for (
const auto &trained_feature : trained_features)
280 n_row +=
static_cast<int> (trained_feature->points.size ());
285 for (std::size_t k = 0; k < trained_features.size (); k++)
288 std::size_t c = hist->
points.size ();
289 for (std::size_t i = 0; i < c; ++i)
290 for (std::size_t j = 0; j < data.cols; ++j)
291 data[(k * c) + i][j] = hist->
points[i].histogram[j];
300 index->buildIndex ();
303 indi.resize (query_features->
points.size ());
304 dist.resize (query_features->
points.size ());
306 for (std::size_t i = 0; i < query_features->
points.size (); i++)
310 memcpy (&p.ptr ()[0], query_features->
points[i].histogram, p.cols * p.rows *
sizeof (float));
314 index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
316 indi[i] = indices[0][0];
317 dist[i] = distances[0][0];
324 delete[] data.ptr ();
328 template <
typename Po
intT>
void
330 std::vector<float> &dist,
332 float feature_threshold,
336 float nfm =
static_cast<float> (n_feature_means);
337 for (std::size_t i = 0; i < out->
points.size (); i++)
339 if (dist[i] < feature_threshold)
341 float l =
static_cast<float> (indi[i]) / nfm;
344 std::modf (l , &intpart);
345 int label =
static_cast<int> (intpart);
347 out->
points[i].label = label+2;
354 template <
typename Po
intT>
void
359 convertCloud (input_cloud_, tmp_cloud);
363 computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
368 kmeansClustering (feature, output, cluster_size_);
372 template <
typename Po
intT>
void
377 std::vector<int> cluster_numbers;
378 findClusters (input_cloud_, cluster_numbers);
379 std::cout <<
"cluster numbers: ";
380 for (
const int &cluster_number : cluster_numbers)
381 std::cout << cluster_number <<
" ";
382 std::cout << std::endl;
384 for (
const int &cluster_number : cluster_numbers)
388 getCloudWithLabel (input_cloud_, label_cloud, cluster_number);
392 computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
396 kmeansClustering (feature, kmeans_feature, cluster_size_);
398 output.push_back (*kmeans_feature);
403 template <
typename Po
intT>
void
406 if (!trained_features_.empty ())
410 convertCloud (input_cloud_, tmp_cloud);
414 computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
417 std::vector<int> indices;
418 std::vector<float> distance;
419 queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
422 int n_feature_means =
static_cast<int> (trained_features_[0]->points.size ());
423 convertCloud (input_cloud_, out);
424 assignLabels (indices, distance, n_feature_means, feature_threshold_, out);
428 PCL_ERROR (
"no training features set \n");
432 #define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier<T>;
434 #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
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 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).
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...
void assignLabels(std::vector< int > &indi, std::vector< float > &dist, int n_feature_means, float feature_threshold, pcl::PointCloud< pcl::PointXYZRGBL >::Ptr out)
std::uint32_t height
The point cloud height (if organized as an image-structure).
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 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...
A point structure representing Euclidean xyz coordinates, and the RGB color.
void computeFPFH(pcl::PointCloud< pcl::PointXYZ >::Ptr in, pcl::PointCloud< pcl::FPFHSignature33 >::Ptr out, float normal_radius_search, float fpfh_radius_search)
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 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)