Point Cloud Library (PCL)  1.12.0-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 #pragma once
40 
41 #include <pcl/gpu/segmentation/gpu_extract_labeled_clusters.h>
42 
43 template <typename PointT> void
45  const pcl::gpu::Octree::Ptr &tree,
46  float tolerance,
47  std::vector<PointIndices> &clusters,
48  unsigned int min_pts_per_cluster,
49  unsigned int max_pts_per_cluster)
50 {
51 
52  // Create a bool vector of processed point indices, and initialize it to false
53  // cloud is a DeviceArray<PointType>
54  std::vector<bool> processed (host_cloud_->size (), false);
55 
56  int max_answers;
57 
58  if(max_pts_per_cluster > host_cloud_->size ())
59  max_answers = static_cast<int> (host_cloud_->size ());
60  else
61  max_answers = max_pts_per_cluster;
62 
63  // to store the current cluster
65 
66  // Process all points in the cloud
67  for (std::size_t i = 0; i < host_cloud_->size (); ++i)
68  {
69  // if we already processed this point continue with the next one
70  if (processed[i])
71  continue;
72  // now we will process this point
73  processed[i] = true;
74 
75  // Create the query queue on the device, point based not indices
76  pcl::gpu::Octree::Queries queries_device;
77  // Create the query queue on the host
79 
80  // Buffer in a new PointXYZ type
81  PointT t = (*host_cloud_)[i];
82  PointXYZ p;
83  p.x = t.x; p.y = t.y; p.z = t.z;
84 
85  // Push the starting point in the vector
86  queries_host.push_back (p);
87  // Clear vector
88  r.indices.clear ();
89  // Push the starting point in
90  r.indices.push_back (static_cast<int> (i));
91 
92  unsigned int found_points = static_cast<unsigned int> (queries_host.size ());
93  unsigned int previous_found_points = 0;
94 
95  pcl::gpu::NeighborIndices result_device;
96 
97  // once the area stop growing, stop also iterating.
98  while (previous_found_points < found_points)
99  {
100  // Move queries to GPU
101  queries_device.upload(queries_host);
102  // Execute search
103  tree->radiusSearch(queries_device, tolerance, max_answers, result_device);
104 
105  // Store the previously found number of points
106  previous_found_points = found_points;
107 
108  // Host buffer for results
109  std::vector<int> sizes, data;
110 
111  // Copy results from GPU to Host
112  result_device.sizes.download (sizes);
113  result_device.data.download (data);
114 
115  for(std::size_t qp = 0; qp < sizes.size (); qp++)
116  {
117  for(int qp_r = 0; qp_r < sizes[qp]; qp_r++)
118  {
119  if(processed[data[qp_r + qp * max_answers]])
120  continue;
121  // Only add if label matches the original label
122  if((*host_cloud_)[i].label == (*host_cloud_)[data[qp_r + qp * max_answers]].label)
123  {
124  processed[data[qp_r + qp * max_answers]] = true;
125  PointT t_l = (*host_cloud_)[data[qp_r + qp * max_answers]];
126  PointXYZ p_l;
127  p_l.x = t_l.x; p_l.y = t_l.y; p_l.z = t_l.z;
128  queries_host.push_back (p_l);
129  found_points++;
130  r.indices.push_back(data[qp_r + qp * max_answers]);
131  }
132  }
133  }
134  }
135  // If this queue is satisfactory, add to the clusters
136  if (found_points >= min_pts_per_cluster && found_points <= max_pts_per_cluster)
137  {
138  std::sort (r.indices.begin (), r.indices.end ());
139  // @todo: check if the following is actually still needed
140  //r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
141 
142  r.header = host_cloud_->header;
143  clusters.push_back (r); // We could avoid a copy by working directly in the vector
144  }
145  }
146 }
147 
148 template <typename PointT> void
150 {
151  // Initialize the GPU search tree
152  if (!tree_)
153  {
154  tree_.reset (new pcl::gpu::Octree());
155  ///@todo what do we do if input isn't a PointXYZ cloud?
156  tree_->setCloud(input_);
157  }
158  if (!tree_->isBuilt())
159  {
160  tree_->build();
161  }
162 /*
163  if(tree_->cloud_.size() != host_cloud.size ())
164  {
165  PCL_ERROR("[pcl::gpu::EuclideanClusterExtraction] size of host cloud and device cloud don't match!\n");
166  return;
167  }
168 */
169  // Extract the actual clusters
170  extractLabeledEuclideanClusters<PointT> (host_cloud_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);
171 
172  // Sort the clusters based on their size (largest one first)
173  std::sort (clusters.rbegin (), clusters.rend (), compareLabeledPointClusters);
174 }
175 
176 #define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::gpu::extractLabeledEuclideanClusters<T> (const typename pcl::PointCloud<T>::Ptr &, const pcl::gpu::Octree::Ptr &,float, std::vector<PointIndices> &, unsigned int, unsigned int);
177 #define PCL_INSTANTIATE_EuclideanLabeledClusterExtraction(T) template class PCL_EXPORTS pcl::gpu::EuclideanLabeledClusterExtraction<T>;
pcl::gpu::NeighborIndices::sizes
DeviceArray< int > sizes
Definition: device_format.hpp:49
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21
pcl::gpu::Octree
Octree implementation on GPU.
Definition: octree.hpp:57
pcl::gpu::NeighborIndices::data
DeviceArray< int > data
Definition: device_format.hpp:48
pcl::PointIndices::header
::pcl::PCLHeader header
Definition: PointIndices.h:19
pcl::PointCloud::VectorType
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:411
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:674
pcl::gpu::NeighborIndices
Definition: device_format.hpp:46
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition: point_types.hpp:346
pcl::gpu::DeviceArray< PointType >
pcl::gpu::extractLabeledEuclideanClusters
void extractLabeledEuclideanClusters(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)
Definition: gpu_extract_labeled_clusters.hpp:44
pcl::PointIndices
Definition: PointIndices.h:11
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
pcl::gpu::compareLabeledPointClusters
bool compareLabeledPointClusters(const pcl::PointIndices &a, const pcl::PointIndices &b)
Sort clusters method (for std::sort).
Definition: gpu_extract_labeled_clusters.h:154
pcl::gpu::DeviceArray::upload
void upload(const T *host_ptr, std::size_t size)
Uploads data to internal buffer in GPU memory.
Definition: device_array.hpp:94
pcl::gpu::Octree::Ptr
shared_ptr< Octree > Ptr
Types.
Definition: octree.hpp:68
pcl::PointCloud::push_back
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::gpu::DeviceArray::download
void download(T *host_ptr) const
Downloads data from internal buffer to CPU memory.
Definition: device_array.hpp:112
pcl::gpu::EuclideanLabeledClusterExtraction::extract
void extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
Definition: gpu_extract_labeled_clusters.hpp:149