Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
43namespace pcl {
44namespace detail {
45
46//// Downloads only the neccssary cluster indices from the device to the host.
47void
49 const pcl::Indices& buffer_indices,
50 std::size_t buffer_size,
51 pcl::Indices& downloaded_indices);
52} // namespace detail
53} // namespace pcl
54
55template <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
185template <typename PointT> void
186pcl::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.
pcl::PCLHeader header
The point cloud header.
void clear()
Removes all points in a cloud and sets the width and height to 0.
std::size_t size() const
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
shared_ptr< PointCloud< PointT > > Ptr
DeviceArray class
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.
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
A point structure representing Euclidean xyz coordinates.
DeviceArray< int > sizes