40 #ifndef PCL_UNARY_CLASSIFIER_HPP_
41 #define PCL_UNARY_CLASSIFIER_HPP_
44 #include <pcl/segmentation/unary_classifier.h>
45 #include <pcl/common/io.h>
46 #include <pcl/kdtree/flann.h>
49 template <
typename Po
intT>
53 normal_radius_search_ (0.01f),
54 fpfh_radius_search_ (0.05f),
55 feature_threshold_ (5.0)
60 template <
typename Po
intT>
66 template <
typename Po
intT>
void
69 if (input_cloud_ != NULL)
70 input_cloud_.reset ();
72 input_cloud_ = input_cloud;
75 std::vector<pcl::PCLPointField> fields;
80 if (label_index != -1)
85 template <
typename Po
intT>
void
91 out->
width =
static_cast<int> (out->
points.size ());
95 for (
size_t i = 0; i < in->
points.size (); i++)
100 point.y = in->
points[i].y;
101 point.z = in->
points[i].z;
106 template <
typename Po
intT>
void
114 out->
width =
static_cast<int> (out->
points.size ());
118 for (
size_t i = 0; i < in->
points.size (); i++)
122 point.x = in->
points[i].x;
123 point.y = in->
points[i].y;
124 point.z = in->
points[i].z;
133 template <
typename Po
intT>
void
135 std::vector<int> &cluster_numbers)
138 std::vector <pcl::PCLPointField> fields;
145 for (
size_t i = 0; i < in->
points.size (); i++)
149 memcpy (&label, reinterpret_cast<char*> (&in->
points[i]) + fields[label_idx].offset,
sizeof(uint32_t));
153 for (
size_t j = 0; j < cluster_numbers.size (); j++)
155 if (static_cast<uint32_t> (cluster_numbers[j]) == label)
162 cluster_numbers.push_back (label);
168 template <
typename Po
intT>
void
174 std::vector <pcl::PCLPointField> fields;
181 for (
size_t i = 0; i < in->
points.size (); i++)
185 memcpy (&label, reinterpret_cast<char*> (&in->
points[i]) + fields[label_idx].offset,
sizeof(uint32_t));
187 if (static_cast<int> (label) == label_num)
191 point.x = in->
points[i].x;
192 point.y = in->
points[i].y;
193 point.z = in->
points[i].z;
194 out->
points.push_back (point);
197 out->
width =
static_cast<int> (out->
points.size ());
204 template <
typename Po
intT>
void
207 float normal_radius_search,
208 float fpfh_radius_search)
233 template <
typename Po
intT>
void
242 for (
size_t i = 0; i < in->
points.size (); i++)
244 std::vector<float> data (33);
245 for (
int idx = 0; idx < 33; idx++)
246 data[idx] = in->
points[i].histogram[idx];
257 out->
width =
static_cast<int> (centroids.size ());
260 out->
points.resize (static_cast<int> (centroids.size ()));
262 for (
size_t i = 0; i < centroids.size (); i++)
265 for (
int idx = 0; idx < 33; idx++)
266 point.
histogram[idx] = centroids[i][idx];
272 template <
typename Po
intT>
void
275 std::vector<int> &indi,
276 std::vector<float> &dist)
280 for (
size_t i = 0; i < trained_features.size (); i++)
281 n_row += static_cast<int> (trained_features[i]->points.size ());
286 for (
size_t k = 0; k < trained_features.size (); k++)
289 size_t c = hist->
points.size ();
290 for (
size_t i = 0; i < c; ++i)
291 for (
size_t j = 0; j < data.cols; ++j)
292 data[(k * c) + i][j] = hist->
points[i].histogram[j];
301 index->buildIndex ();
304 indi.resize (query_features->
points.size ());
305 dist.resize (query_features->
points.size ());
307 for (
size_t i = 0; i < query_features->
points.size (); i++)
311 memcpy (&p.ptr ()[0], query_features->
points[i].histogram, p.cols * p.rows *
sizeof (float));
315 index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
317 indi[i] = indices[0][0];
318 dist[i] = distances[0][0];
325 delete[] data.ptr ();
329 template <
typename Po
intT>
void
331 std::vector<float> &dist,
333 float feature_threshold,
337 float nfm =
static_cast<float> (n_feature_means);
338 for (
size_t i = 0; i < out->
points.size (); i++)
340 if (dist[i] < feature_threshold)
342 float l =
static_cast<float> (indi[i]) / nfm;
345 std::modf (l , &intpart);
346 int label =
static_cast<int> (intpart);
348 out->
points[i].label = label+2;
355 template <
typename Po
intT>
void
360 convertCloud (input_cloud_, tmp_cloud);
364 computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
369 kmeansClustering (feature, output, cluster_size_);
373 template <
typename Po
intT>
void
378 std::vector<int> cluster_numbers;
379 findClusters (input_cloud_, cluster_numbers);
380 std::cout <<
"cluster numbers: ";
381 for (
size_t i = 0; i < cluster_numbers.size (); i++)
382 std::cout << cluster_numbers[i] <<
" ";
383 std::cout << std::endl;
385 for (
size_t i = 0; i < cluster_numbers.size (); i++)
389 getCloudWithLabel (input_cloud_, label_cloud, cluster_numbers[i]);
393 computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_);
397 kmeansClustering (feature, kmeans_feature, cluster_size_);
399 output.push_back (*kmeans_feature);
404 template <
typename Po
intT>
void
407 if (trained_features_.size () > 0)
411 convertCloud (input_cloud_, tmp_cloud);
415 computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
418 std::vector<int> indices;
419 std::vector<float> distance;
420 queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
423 int n_feature_means =
static_cast<int> (trained_features_[0]->points.size ());
424 convertCloud (input_cloud_, out);
425 assignLabels (indices, distance, n_feature_means, feature_threshold_, out);
429 PCL_ERROR (
"no training features set \n");
433 #define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier<T>;
435 #endif // PCL_UNARY_CLASSIFIER_HPP_
std::vector< Point > Centroids
boost::shared_ptr< PointCloud< PointT > > Ptr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
void addDataPoint(Point &data_point)
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
uint32_t width
The point cloud width (if organized as an image-structure).
This file defines compatibility wrappers for low level I/O functions.
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::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setClusterSize(unsigned int k)
This method sets the k-means cluster size.
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
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)
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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.
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 setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
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)