Point Cloud Library (PCL)  1.12.0
gpu_extract_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  * @author: Koen Buys
37  */
38 
39 #pragma once
40 #include <pcl/common/copy_point.h>
41 #include <pcl/gpu/segmentation/gpu_extract_clusters.h>
42 
43 namespace pcl {
44 namespace detail {
45 
46 //// Downloads only the neccssary cluster indices from the device to the host.
47 void
49  const pcl::Indices& buffer_indices,
50  std::size_t buffer_size,
51  pcl::Indices& downloaded_indices);
52 } // namespace detail
53 } // namespace pcl
54 
55 template <typename PointT> void
57  const pcl::gpu::Octree::Ptr &tree,
58  float tolerance,
59  std::vector<PointIndices> &clusters,
60  unsigned int min_pts_per_cluster,
61  unsigned int max_pts_per_cluster)
62 {
63 
64  // Create a bool vector of processed point indices, and initialize it to false
65  // cloud is a DeviceArray<PointType>
66  PCL_DEBUG("[pcl::gpu::extractEuclideanClusters]\n");
67  std::vector<bool> processed (host_cloud_->size (), false);
68 
69  int max_answers;
70 
71  if(max_pts_per_cluster > host_cloud_->size())
72  max_answers = host_cloud_->size();
73  else
74  max_answers = max_pts_per_cluster;
75  PCL_DEBUG("Max_answers: %i\n", max_answers);
76 
77  // to store the current cluster
79 
80  DeviceArray<PointXYZ> queries_device_buffer;
81  queries_device_buffer.create(max_answers);
82 
83  // Host buffer for results
84  pcl::Indices sizes, data, cpu_tmp;
85  // Process all points in the cloud
86  for (std::size_t i = 0; i < host_cloud_->size (); ++i)
87  {
88  // if we already processed this point continue with the next one
89  if (processed[i])
90  continue;
91  // now we will process this point
92  processed[i] = true;
93 
94  // Create the query queue on the device, point based not indices
95  pcl::gpu::Octree::Queries queries_device;
96  // Create the query queue on the host
98 
99  // Buffer in a new PointXYZ type
100  PointXYZ p;
101  pcl::copyPoint((*host_cloud_)[i], p);
102 
103  // Push the starting point in the vector
104  queries_host.push_back (p);
105  // Clear vector
106  r.indices.clear();
107  // Push the starting point in
108  r.indices.push_back(i);
109 
110  unsigned int found_points = queries_host.size ();
111  unsigned int previous_found_points = 0;
112 
113  pcl::gpu::NeighborIndices result_device;
114 
115  // once the area stop growing, stop also iterating.
116  do
117  {
118  data.clear();
119  // if the number of queries is not high enough implement search on Host here
120  if(queries_host.size () <= 10) ///@todo: adjust this to a variable number settable with method
121  {
122  PCL_DEBUG(" CPU: ");
123  for(std::size_t p = 0; p < queries_host.size (); p++)
124  {
125  // Execute the radiusSearch on the host
126  cpu_tmp.clear();
127  tree->radiusSearchHost(queries_host[p], tolerance, cpu_tmp, max_answers);
128  std::copy(cpu_tmp.begin(), cpu_tmp.end(), std::back_inserter(data));
129  }
130  }
131  // If number of queries is high enough do it here
132  else
133  {
134  PCL_DEBUG(" GPU: ");
135  sizes.clear();
136  // Copy buffer
137  queries_device = DeviceArray<PointXYZ>(queries_device_buffer.ptr(),queries_host.size());
138  // Move queries to GPU
139  queries_device.upload(queries_host);
140  // Execute search
141  tree->radiusSearch(queries_device, tolerance, max_answers, result_device);
142  // Copy results from GPU to Host
143  result_device.sizes.download(sizes);
144  pcl::detail::economical_download(result_device, sizes, max_answers, data);
145  }
146  // Store the previously found number of points
147  previous_found_points = found_points;
148  // Clear queries list
149  queries_host.clear();
150 
151  if(data.size () == 1)
152  continue;
153 
154  // Process the results
155  for(auto idx : data)
156  {
157  if(processed[idx])
158  continue;
159  processed[idx] = true;
160  PointXYZ p;
161  pcl::copyPoint((*host_cloud_)[idx], p);
162  queries_host.push_back (p);
163  found_points++;
164  r.indices.push_back(idx);
165  }
166  PCL_DEBUG(" data.size: %i, foundpoints: %i, previous: %i", data.size() ,
167  found_points, previous_found_points);
168  PCL_DEBUG(" new points: %i, next queries size: %i\n", found_points - previous_found_points,
169  queries_host.size());
170  }
171  while (previous_found_points < found_points);
172  // If this queue is satisfactory, add to the clusters
173  if (found_points >= min_pts_per_cluster && found_points <= max_pts_per_cluster)
174  {
175  std::sort (r.indices.begin (), r.indices.end ());
176  // @todo: check if the following is actually still needed
177  //r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
178 
179  r.header = host_cloud_->header;
180  clusters.push_back (r); // We could avoid a copy by working directly in the vector
181  }
182  }
183 }
184 
185 template <typename PointT> void
186 pcl::gpu::EuclideanClusterExtraction<PointT>::extract (std::vector<pcl::PointIndices> &clusters)
187 {
188 /*
189  // Initialize the GPU search tree
190  if (!tree_)
191  {
192  tree_.reset (new pcl::gpu::Octree());
193  ///@todo what do we do if input isn't a PointXYZ cloud?
194  tree_.setCloud(input_);
195  }
196 */
197  if (!tree_->isBuilt())
198  {
199  tree_->build();
200  }
201 /*
202  if(tree_->cloud_.size() != host_cloud.size ())
203  {
204  PCL_ERROR("[pcl::gpu::EuclideanClusterExtraction] size of host cloud and device cloud don't match!\n");
205  return;
206  }
207 */
208  // Extract the actual clusters
209  extractEuclideanClusters<PointT> (host_cloud_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);
210  PCL_DEBUG("INFO: end of extractEuclideanClusters\n");
211  // Sort the clusters based on their size (largest one first)
212  //std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);
213 }
214 
215 #define PCL_INSTANTIATE_extractEuclideanClusters(T) template void PCL_EXPORTS pcl::gpu::extractEuclideanClusters<T> (const typename pcl::PointCloud<T>::Ptr &, const pcl::gpu::Octree::Ptr &,float, std::vector<PointIndices> &, unsigned int, unsigned int);
216 #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::gpu::EuclideanClusterExtraction<T>;
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:652
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:874
std::size_t size() const
Definition: point_cloud.h:443
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:411
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
DeviceArray class
Definition: device_array.h:54
void upload(const T *host_ptr, std::size_t size)
Uploads data to internal buffer in GPU memory.
void download(T *host_ptr) const
Downloads data from internal buffer to CPU memory.
void create(std::size_t size)
Allocates internal buffer in GPU memory.
T * ptr()
Returns pointer for internal buffer in GPU memory.
void extract(std::vector< pcl::PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
shared_ptr< Octree > Ptr
Types.
Definition: octree.hpp:68
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:137
void economical_download(const pcl::gpu::NeighborIndices &source_indices, const pcl::Indices &buffer_indices, std::size_t buffer_size, pcl::Indices &downloaded_indices)
void extractEuclideanClusters(const typename pcl::PointCloud< PointT >::Ptr &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)
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
::pcl::PCLHeader header
Definition: PointIndices.h:19
A point structure representing Euclidean xyz coordinates.
DeviceArray< int > sizes