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;
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>; 436 #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...
float distance(const PointT &p1, const PointT &p2)
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 <setInputCloud (), setIndices ()> 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)