Point Cloud Library (PCL)  1.13.1-dev
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>
56 void
58  const typename pcl::PointCloud<PointT>::Ptr& host_cloud_,
59  const pcl::gpu::Octree::Ptr& tree,
60  float tolerance,
61  std::vector<PointIndices>& clusters,
62  unsigned int min_pts_per_cluster,
63  unsigned int max_pts_per_cluster)
64 {
65 
66  // Create a bool vector of processed point indices, and initialize it to false
67  // cloud is a DeviceArray<PointType>
68  PCL_DEBUG("[pcl::gpu::extractEuclideanClusters]\n");
69  std::vector<bool> processed(host_cloud_->size(), false);
70 
71  int max_answers;
72 
73  if (max_pts_per_cluster > host_cloud_->size())
74  max_answers = host_cloud_->size();
75  else
76  max_answers = max_pts_per_cluster;
77  PCL_DEBUG("Max_answers: %i\n", max_answers);
78 
79  // to store the current cluster
81 
82  DeviceArray<PointXYZ> queries_device_buffer;
83  queries_device_buffer.create(max_answers);
84 
85  // Host buffer for results
86  pcl::Indices sizes, data, cpu_tmp;
87  // Process all points in the cloud
88  for (std::size_t i = 0; i < host_cloud_->size(); ++i) {
89  // if we already processed this point continue with the next one
90  if (processed[i])
91  continue;
92  // now we will process this point
93  processed[i] = true;
94 
95  // Create the query queue on the device, point based not indices
96  pcl::gpu::Octree::Queries queries_device;
97  // Create the query queue on the host
99 
100  // Buffer in a new PointXYZ type
101  PointXYZ p;
102  pcl::copyPoint((*host_cloud_)[i], p);
103 
104  // Push the starting point in the vector
105  queries_host.push_back(p);
106  // Clear vector
107  r.indices.clear();
108  // Push the starting point in
109  r.indices.push_back(i);
110 
111  unsigned int found_points = queries_host.size();
112  unsigned int previous_found_points = 0;
113 
114  pcl::gpu::NeighborIndices result_device;
115 
116  // once the area stop growing, stop also iterating.
117  do {
118  data.clear();
119  // if the number of queries is not high enough implement search on Host here
120  if (queries_host.size() <=
121  10) ///@todo: adjust this to a variable number settable with method
122  {
123  PCL_DEBUG(" CPU: ");
124  for (std::size_t p = 0; p < queries_host.size(); p++) {
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  PCL_DEBUG(" GPU: ");
134  sizes.clear();
135  // Copy buffer
136  queries_device =
137  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  if (processed[idx])
157  continue;
158  processed[idx] = true;
159  PointXYZ p;
160  pcl::copyPoint((*host_cloud_)[idx], p);
161  queries_host.push_back(p);
162  found_points++;
163  r.indices.push_back(idx);
164  }
165  PCL_DEBUG(" data.size: %i, foundpoints: %i, previous: %i",
166  data.size(),
167  found_points,
168  previous_found_points);
169  PCL_DEBUG(" new points: %i, next queries size: %i\n",
170  found_points - previous_found_points,
171  queries_host.size());
172  } while (previous_found_points < found_points);
173  // If this queue is satisfactory, add to the clusters
174  if (found_points >= min_pts_per_cluster && found_points <= max_pts_per_cluster) {
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 ()),
178  // r.indices.end ());
179 
180  r.header = host_cloud_->header;
181  clusters.push_back(r); // We could avoid a copy by working directly in the vector
182  }
183  }
184 }
185 
186 template <typename PointT>
187 void
189  std::vector<pcl::PointIndices>& clusters)
190 {
191  /*
192  // Initialize the GPU search tree
193  if (!tree_)
194  {
195  tree_.reset (new pcl::gpu::Octree());
196  ///@todo what do we do if input isn't a PointXYZ cloud?
197  tree_.setCloud(input_);
198  }
199  */
200  if (!tree_->isBuilt()) {
201  tree_->build();
202  }
203  /*
204  if(tree_->cloud_.size() != host_cloud.size ())
205  {
206  PCL_ERROR("[pcl::gpu::EuclideanClusterExtraction] size of host cloud and device
207  cloud don't match!\n"); return;
208  }
209  */
210  // Extract the actual clusters
211  extractEuclideanClusters<PointT>(host_cloud_,
212  tree_,
213  cluster_tolerance_,
214  clusters,
215  min_pts_per_cluster_,
216  max_pts_per_cluster_);
217  PCL_DEBUG("INFO: end of extractEuclideanClusters\n");
218  // Sort the clusters based on their size (largest one first)
219  // std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);
220 }
221 
222 #define PCL_INSTANTIATE_extractEuclideanClusters(T) \
223  template void PCL_EXPORTS pcl::gpu::extractEuclideanClusters<T>( \
224  const typename pcl::PointCloud<T>::Ptr&, \
225  const pcl::gpu::Octree::Ptr&, \
226  float, \
227  std::vector<PointIndices>&, \
228  unsigned int, \
229  unsigned int);
230 #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) \
231  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:663
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:885
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)
extract clusters of a PointCloud given by <setInputCloud(), setIndices()>
shared_ptr< Octree > Ptr
Types.
Definition: octree.hpp:69
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:18
A point structure representing Euclidean xyz coordinates.
DeviceArray< int > sizes