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)
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;
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>;
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.
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, std::vector< int > &indi, std::vector< float > &dist)
void assignLabels(std::vector< int > &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)
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.