Point Cloud Library (PCL)  1.8.1-dev
gpu_extract_labeled_clusters.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * $Id:$
36  *
37  */
38 
39 #ifndef PCL_GPU_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_
40 #define PCL_GPU_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_
41 
42 #include <pcl/gpu/segmentation/gpu_extract_labeled_clusters.h>
43 
44 template <typename PointT> void
46  const pcl::gpu::Octree::Ptr &tree,
47  float tolerance,
48  std::vector<PointIndices> &clusters,
49  unsigned int min_pts_per_cluster,
50  unsigned int max_pts_per_cluster)
51 {
52 
53  // Create a bool vector of processed point indices, and initialize it to false
54  // cloud is a DeviceArray<PointType>
55  std::vector<bool> processed (host_cloud_->points.size (), false);
56 
57  int max_answers;
58 
59  if(max_pts_per_cluster > host_cloud_->points.size ())
60  max_answers = static_cast<int> (host_cloud_->points.size ());
61  else
62  max_answers = max_pts_per_cluster;
63 
64  // to store the current cluster
66 
67  // Process all points in the cloud
68  for (size_t i = 0; i < host_cloud_->points.size (); ++i)
69  {
70  // if we already processed this point continue with the next one
71  if (processed[i])
72  continue;
73  // now we will process this point
74  processed[i] = true;
75 
76  // Create the query queue on the device, point based not indices
77  pcl::gpu::Octree::Queries queries_device;
78  // Create the query queue on the host
80 
81  // Buffer in a new PointXYZ type
82  PointT t = host_cloud_->points[i];
83  PointXYZ p;
84  p.x = t.x; p.y = t.y; p.z = t.z;
85 
86  // Push the starting point in the vector
87  queries_host.push_back (p);
88  // Clear vector
89  r.indices.clear ();
90  // Push the starting point in
91  r.indices.push_back (static_cast<int> (i));
92 
93  unsigned int found_points = static_cast<unsigned int> (queries_host.size ());
94  unsigned int previous_found_points = 0;
95 
96  pcl::gpu::NeighborIndices result_device;
97 
98  // once the area stop growing, stop also iterating.
99  while (previous_found_points < found_points)
100  {
101  // Move queries to GPU
102  queries_device.upload(queries_host);
103  // Execute search
104  tree->radiusSearch(queries_device, tolerance, max_answers, result_device);
105 
106  // Store the previously found number of points
107  previous_found_points = found_points;
108 
109  // Host buffer for results
110  std::vector<int> sizes, data;
111 
112  // Copy results from GPU to Host
113  result_device.sizes.download (sizes);
114  result_device.data.download (data);
115 
116  for(size_t qp = 0; qp < sizes.size (); qp++)
117  {
118  for(int qp_r = 0; qp_r < sizes[qp]; qp_r++)
119  {
120  if(processed[data[qp_r + qp * max_answers]])
121  continue;
122  // Only add if label matches the original label
123  if(host_cloud_->points[i].label == host_cloud_->points[data[qp_r + qp * max_answers]].label)
124  {
125  processed[data[qp_r + qp * max_answers]] = true;
126  PointT t_l = host_cloud_->points[data[qp_r + qp * max_answers]];
127  PointXYZ p_l;
128  p_l.x = t_l.x; p_l.y = t_l.y; p_l.z = t_l.z;
129  queries_host.push_back (p_l);
130  found_points++;
131  r.indices.push_back(data[qp_r + qp * max_answers]);
132  }
133  }
134  }
135  }
136  // If this queue is satisfactory, add to the clusters
137  if (found_points >= min_pts_per_cluster && found_points <= max_pts_per_cluster)
138  {
139  std::sort (r.indices.begin (), r.indices.end ());
140  // @todo: check if the following is actually still needed
141  //r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
142 
143  r.header = host_cloud_->header;
144  clusters.push_back (r); // We could avoid a copy by working directly in the vector
145  }
146  }
147 }
148 
149 template <typename PointT> void
151 {
152  // Initialize the GPU search tree
153  if (!tree_)
154  {
155  tree_.reset (new pcl::gpu::Octree());
156  ///@todo what do we do if input isn't a PointXYZ cloud?
157  tree_->setCloud(input_);
158  }
159  if (!tree_->isBuilt())
160  {
161  tree_->build();
162  }
163 /*
164  if(tree_->cloud_.size() != host_cloud.points.size ())
165  {
166  PCL_ERROR("[pcl::gpu::EuclideanClusterExtraction] size of host cloud and device cloud don't match!\n");
167  return;
168  }
169 */
170  // Extract the actual clusters
171  extractLabeledEuclideanClusters (host_cloud_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);
172 
173  // Sort the clusters based on their size (largest one first)
174  std::sort (clusters.rbegin (), clusters.rend (), compareLabeledPointClusters);
175 }
176 
177 #define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::gpu::extractLabeledEuclideanClusters (const boost::shared_ptr<pcl::PointCloud<T> > &, const pcl::gpu::Octree::Ptr &,float, std::vector<PointIndices> &, unsigned int, unsigned int);
178 #define PCL_INSTANTIATE_EuclideanLabeledClusterExtraction(T) template class PCL_EXPORTS pcl::gpu::EuclideanLabeledClusterExtraction<T>;
179 /*
180 #define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::gpu::extractLabeledEuclideanClusters (const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBL> > &, const pcl::gpu::Octree::Ptr &,float, std::vector<PointIndices> &, unsigned int, unsigned int);
181 #define PCL_INSTANTIATE_EuclideanLabeledClusterExtraction(T) template class PCL_EXPORTS pcl::gpu::EuclideanLabeledClusterExtraction<T>;
182 */
183 #endif //PCL_GPU_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:426
boost::shared_ptr< Octree > Ptr
Types.
Definition: octree.hpp:67
std::vector< int > indices
Definition: PointIndices.h:19
void download(T *host_ptr) const
Downloads data from internal buffer to CPU memory.
Octree implementation on GPU.
Definition: octree.hpp:56
void extractLabeledEuclideanClusters(const boost::shared_ptr< pcl::PointCloud< PointT > > &host_cloud_, const pcl::gpu::Octree::Ptr &tree, float tolerance, std::vector< PointIndices > &clusters, unsigned int min_pts_per_cluster, unsigned int max_pts_per_cluster)
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:480
bool compareLabeledPointClusters(const pcl::PointIndices &a, const pcl::PointIndices &b)
Sort clusters method (for std::sort).
DeviceArray< int > data
A point structure representing Euclidean xyz coordinates.
::pcl::PCLHeader header
Definition: PointIndices.h:17
size_t size() const
Definition: point_cloud.h:448
void extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
DeviceArray< int > sizes
A point structure representing Euclidean xyz coordinates, and the RGB color.
void upload(const T *host_ptr, size_t size)
Uploads data to internal buffer in GPU memory.
void extractLabeledEuclideanClusters(const PointCloud< PointT > &cloud, const boost::shared_ptr< search::Search< PointT > > &tree, float tolerance, std::vector< std::vector< PointIndices > > &labeled_clusters, unsigned int min_pts_per_cluster=1, unsigned int max_pts_per_cluster=std::numeric_limits< unsigned int >::max(), unsigned int max_label=std::numeric_limits< unsigned int >::max())
Decompose a region of space into clusters based on the Euclidean distance between points...