Point Cloud Library (PCL)  1.14.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 #include <pcl/pcl_exports.h>
43 
44 namespace pcl {
45 namespace detail {
46 
47 //// Downloads only the neccssary cluster indices from the device to the host.
48 PCL_EXPORTS void
50  const pcl::Indices& buffer_indices,
51  std::size_t buffer_size,
52  pcl::Indices& downloaded_indices);
53 } // namespace detail
54 } // namespace pcl
55 
56 template <typename PointT>
57 void
59  const typename pcl::PointCloud<PointT>::Ptr& host_cloud_,
60  const pcl::gpu::Octree::Ptr& tree,
61  float tolerance,
62  std::vector<PointIndices>& clusters,
63  unsigned int min_pts_per_cluster,
64  unsigned int max_pts_per_cluster)
65 {
66 
67  // Create a bool vector of processed point indices, and initialize it to false
68  // cloud is a DeviceArray<PointType>
69  PCL_DEBUG("[pcl::gpu::extractEuclideanClusters]\n");
70  std::vector<bool> processed(host_cloud_->size(), false);
71 
72  int max_answers;
73 
74  if (max_pts_per_cluster > host_cloud_->size())
75  max_answers = host_cloud_->size();
76  else
77  max_answers = max_pts_per_cluster;
78  PCL_DEBUG("Max_answers: %i\n", max_answers);
79 
80  // to store the current cluster
82 
83  DeviceArray<PointXYZ> queries_device_buffer;
84  queries_device_buffer.create(max_answers);
85 
86  // Host buffer for results
87  pcl::Indices sizes, data, cpu_tmp;
88  // Process all points in the cloud
89  for (std::size_t i = 0; i < host_cloud_->size(); ++i) {
90  // if we already processed this point continue with the next one
91  if (processed[i])
92  continue;
93  // now we will process this point
94  processed[i] = true;
95 
96  // Create the query queue on the device, point based not indices
97  pcl::gpu::Octree::Queries queries_device;
98  // Create the query queue on the host
100 
101  // Buffer in a new PointXYZ type
102  PointXYZ p;
103  pcl::copyPoint((*host_cloud_)[i], p);
104 
105  // Push the starting point in the vector
106  queries_host.push_back(p);
107  // Clear vector
108  r.indices.clear();
109  // Push the starting point in
110  r.indices.push_back(i);
111 
112  unsigned int found_points = queries_host.size();
113  unsigned int previous_found_points = 0;
114 
115  pcl::gpu::NeighborIndices result_device;
116 
117  // once the area stop growing, stop also iterating.
118  do {
119  data.clear();
120  // if the number of queries is not high enough implement search on Host here
121  if (queries_host.size() <=
122  10) ///@todo: adjust this to a variable number settable with method
123  {
124  PCL_DEBUG(" CPU: ");
125  for (std::size_t p = 0; p < queries_host.size(); p++) {
126  // Execute the radiusSearch on the host
127  cpu_tmp.clear();
128  tree->radiusSearchHost(queries_host[p], tolerance, cpu_tmp, max_answers);
129  std::copy(cpu_tmp.begin(), cpu_tmp.end(), std::back_inserter(data));
130  }
131  }
132  // If number of queries is high enough do it here
133  else {
134  PCL_DEBUG(" GPU: ");
135  sizes.clear();
136  // Copy buffer
137  queries_device =
138  DeviceArray<PointXYZ>(queries_device_buffer.ptr(), queries_host.size());
139  // Move queries to GPU
140  queries_device.upload(queries_host);
141  // Execute search
142  tree->radiusSearch(queries_device, tolerance, max_answers, result_device);
143  // Copy results from GPU to Host
144  result_device.sizes.download(sizes);
145  pcl::detail::economical_download(result_device, sizes, max_answers, data);
146  }
147  // Store the previously found number of points
148  previous_found_points = found_points;
149  // Clear queries list
150  queries_host.clear();
151 
152  if (data.size() == 1)
153  continue;
154 
155  // Process the results
156  for (const auto& idx : data) {
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",
167  data.size(),
168  found_points,
169  previous_found_points);
170  PCL_DEBUG(" new points: %i, next queries size: %i\n",
171  found_points - previous_found_points,
172  queries_host.size());
173  } while (previous_found_points < found_points);
174  // If this queue is satisfactory, add to the clusters
175  if (found_points >= min_pts_per_cluster && found_points <= max_pts_per_cluster) {
176  std::sort(r.indices.begin(), r.indices.end());
177  // @todo: check if the following is actually still needed
178  // r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()),
179  // r.indices.end ());
180 
181  r.header = host_cloud_->header;
182  clusters.push_back(r); // We could avoid a copy by working directly in the vector
183  }
184  }
185 }
186 
187 template <typename PointT>
188 void
190  std::vector<pcl::PointIndices>& clusters)
191 {
192  /*
193  // Initialize the GPU search tree
194  if (!tree_)
195  {
196  tree_.reset (new pcl::gpu::Octree());
197  ///@todo what do we do if input isn't a PointXYZ cloud?
198  tree_.setCloud(input_);
199  }
200  */
201  if (!tree_->isBuilt()) {
202  tree_->build();
203  }
204  /*
205  if(tree_->cloud_.size() != host_cloud.size ())
206  {
207  PCL_ERROR("[pcl::gpu::EuclideanClusterExtraction] size of host cloud and device
208  cloud don't match!\n"); return;
209  }
210  */
211  // Extract the actual clusters
212  extractEuclideanClusters<PointT>(host_cloud_,
213  tree_,
214  cluster_tolerance_,
215  clusters,
216  min_pts_per_cluster_,
217  max_pts_per_cluster_);
218  PCL_DEBUG("INFO: end of extractEuclideanClusters\n");
219  // Sort the clusters based on their size (largest one first)
220  // std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);
221 }
222 
223 #define PCL_INSTANTIATE_extractEuclideanClusters(T) \
224  template void PCL_EXPORTS pcl::gpu::extractEuclideanClusters<T>( \
225  const typename pcl::PointCloud<T>::Ptr&, \
226  const pcl::gpu::Octree::Ptr&, \
227  float, \
228  std::vector<PointIndices>&, \
229  unsigned int, \
230  unsigned int);
231 #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) \
232  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
PCL_EXPORTS 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
#define PCL_EXPORTS
Definition: pcl_macros.h:325
::pcl::PCLHeader header
Definition: PointIndices.h:18
A point structure representing Euclidean xyz coordinates.
DeviceArray< int > sizes