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 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 373 template <
typename Po
intT>
void 378 std::vector<int> 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++)
399 output.push_back (*kmeans_feature);
404 template <
typename Po
intT>
void 418 std::vector<int> indices;
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
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)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void convertCloud(typename pcl::PointCloud< PointT >::Ptr in, pcl::PointCloud< pcl::PointXYZ >::Ptr out)
unsigned int cluster_size_
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)
std::vector< pcl::PointCloud< pcl::FPFHSignature33 >::Ptr > trained_features_
A point structure representing the Fast Point Feature Histogram (FPFH).
uint32_t height
The point cloud height (if organized as an image-structure).
boost::shared_ptr< PointCloud< PointT > > Ptr
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.
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.
pcl::PointCloud< PointT >::Ptr input_cloud_
Contains 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)
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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)
float fpfh_radius_search_
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).
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 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.
float normal_radius_search_
void getCloudWithLabel(typename pcl::PointCloud< PointT >::Ptr in, pcl::PointCloud< pcl::PointXYZ >::Ptr out, int label_num)