Point Cloud Library (PCL)  1.9.0-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 #ifndef PCL_GPU_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
40 #define PCL_GPU_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
41 
42 #include <pcl/gpu/segmentation/gpu_extract_clusters.h>
43 
44 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 = host_cloud_->points.size();
61  else
62  max_answers = max_pts_per_cluster;
63  std::cout << "Max_answers: " << max_answers << std::endl;
64 
65  // to store the current cluster
67 
68  DeviceArray<PointXYZ> queries_device_buffer;
69  queries_device_buffer.create(max_answers);
70 
71  // Process all points in the cloud
72  for (size_t i = 0; i < host_cloud_->points.size (); ++i)
73  {
74  // if we already processed this point continue with the next one
75  if (processed[i])
76  continue;
77  // now we will process this point
78  processed[i] = true;
79 
80  // Create the query queue on the device, point based not indices
81  pcl::gpu::Octree::Queries queries_device;
82  // Create the query queue on the host
84  // Push the starting point in the vector
85  queries_host.push_back (host_cloud_->points[i]);
86  // Clear vector
87  r.indices.clear();
88  // Push the starting point in
89  r.indices.push_back(i);
90 
91  unsigned int found_points = queries_host.size ();
92  unsigned int previous_found_points = 0;
93 
94  pcl::gpu::NeighborIndices result_device;
95 
96  // once the area stop growing, stop also iterating.
97  do
98  {
99  // Host buffer for results
100  std::vector<int> sizes, data;
101 
102  // if the number of queries is not high enough implement search on Host here
103  if(queries_host.size () <= 10) ///@todo: adjust this to a variable number settable with method
104  {
105  std::cout << " CPU: ";
106  for(size_t p = 0; p < queries_host.size (); p++)
107  {
108  // Execute the radiusSearch on the host
109  tree->radiusSearchHost(queries_host[p], tolerance, data, max_answers);
110  }
111  // Store the previously found number of points
112  previous_found_points = found_points;
113  // Clear queries list
114  queries_host.clear();
115 
116  //std::unique(data.begin(), data.end());
117  if(data.size () == 1)
118  continue;
119 
120  // Process the results
121  for(size_t i = 0; i < data.size (); i++)
122  {
123  if(processed[data[i]])
124  continue;
125  processed[data[i]] = true;
126  queries_host.push_back (host_cloud_->points[data[i]]);
127  found_points++;
128  r.indices.push_back(data[i]);
129  }
130  }
131 
132  // If number of queries is high enough do it here
133  else
134  {
135  std::cout << " GPU: ";
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  result_device.data.download (data);
145  // Store the previously found number of points
146  previous_found_points = found_points;
147  // Clear queries list
148  queries_host.clear();
149  for(size_t qp = 0; qp < sizes.size (); qp++)
150  {
151  for(int qp_r = 0; qp_r < sizes[qp]; qp_r++)
152  {
153  if(processed[data[qp_r + qp * max_answers]])
154  continue;
155  processed[data[qp_r + qp * max_answers]] = true;
156  queries_host.push_back (host_cloud_->points[data[qp_r + qp * max_answers]]);
157  found_points++;
158  r.indices.push_back(data[qp_r + qp * max_answers]);
159  }
160  }
161  }
162  std::cout << " data.size: " << data.size() << " foundpoints: " << found_points << " previous: " << previous_found_points;
163  std::cout << " new points: " << found_points - previous_found_points << " next queries size: " << queries_host.size() << std::endl;
164  }
165  while (previous_found_points < found_points);
166  // If this queue is satisfactory, add to the clusters
167  if (found_points >= min_pts_per_cluster && found_points <= max_pts_per_cluster)
168  {
169  std::sort (r.indices.begin (), r.indices.end ());
170  // @todo: check if the following is actually still needed
171  //r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
172 
173  r.header = host_cloud_->header;
174  clusters.push_back (r); // We could avoid a copy by working directly in the vector
175  }
176  }
177 }
178 
179 void
180 pcl::gpu::EuclideanClusterExtraction::extract (std::vector<pcl::PointIndices> &clusters)
181 {
182 /*
183  // Initialize the GPU search tree
184  if (!tree_)
185  {
186  tree_.reset (new pcl::gpu::Octree());
187  ///@todo what do we do if input isn't a PointXYZ cloud?
188  tree_.setCloud(input_);
189  }
190 */
191  if (!tree_->isBuilt())
192  {
193  tree_->build();
194  }
195 /*
196  if(tree_->cloud_.size() != host_cloud.points.size ())
197  {
198  PCL_ERROR("[pcl::gpu::EuclideanClusterExtraction] size of host cloud and device cloud don't match!\n");
199  return;
200  }
201 */
202  // Extract the actual clusters
204  std::cout << "INFO: end of extractEuclideanClusters " << std::endl;
205  // Sort the clusters based on their size (largest one first)
206  //std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);
207 }
208 
209 #endif //PCL_GPU_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
double cluster_tolerance_
The spatial cluster tolerance as a measure in the L2 Euclidean space.
size_t size() const
Definition: point_cloud.h:448
GPUTreePtr tree_
A pointer to the spatial search object.
boost::shared_ptr< Octree > Ptr
Types.
Definition: octree.hpp:67
int max_pts_per_cluster_
The maximum number of points that a cluster needs to contain in order to be considered valid (default...
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:426
std::vector< int > indices
Definition: PointIndices.h:19
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:480
void extract(std::vector< pcl::PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
void download(T *host_ptr) const
Downloads data from internal buffer to CPU memory.
PointCloudHostPtr host_cloud_
the original cloud the Host
int min_pts_per_cluster_
The minimum number of points that a cluster needs to contain in order to be considered valid (default...
DeviceArray class
Definition: device_array.h:57
void create(size_t size)
Allocates internal buffer in GPU memory.
DeviceArray< int > data
::pcl::PCLHeader header
Definition: PointIndices.h:17
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:576
DeviceArray< int > sizes
T * ptr()
Returns pointer for internal buffer in GPU memory.
void extractEuclideanClusters(const boost::shared_ptr< pcl::PointCloud< pcl::PointXYZ > > &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 upload(const T *host_ptr, size_t size)
Uploads data to internal buffer in GPU memory.