Point Cloud Library (PCL) 1.15.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#include <pcl/pcl_exports.h>
43
44namespace pcl {
45namespace detail {
46
47//// Downloads only the neccssary cluster indices from the device to the host.
48PCL_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
56template <typename PointT>
57void
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
187template <typename PointT>
188void
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.
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)
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.
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
::pcl::PCLHeader header
A point structure representing Euclidean xyz coordinates.
DeviceArray< int > sizes