Point Cloud Library (PCL) 1.14.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>
56void
58 const typename pcl::PointCloud<PointT>::Ptr& host_cloud_,
59 const pcl::gpu::Octree::Ptr& tree,
60 float tolerance,
61 std::vector<PointIndices>& clusters,
62 unsigned int min_pts_per_cluster,
63 unsigned int max_pts_per_cluster)
64{
65
66 // Create a bool vector of processed point indices, and initialize it to false
67 // cloud is a DeviceArray<PointType>
68 PCL_DEBUG("[pcl::gpu::extractEuclideanClusters]\n");
69 std::vector<bool> processed(host_cloud_->size(), false);
70
71 int max_answers;
72
73 if (max_pts_per_cluster > host_cloud_->size())
74 max_answers = host_cloud_->size();
75 else
76 max_answers = max_pts_per_cluster;
77 PCL_DEBUG("Max_answers: %i\n", max_answers);
78
79 // to store the current cluster
81
82 DeviceArray<PointXYZ> queries_device_buffer;
83 queries_device_buffer.create(max_answers);
84
85 // Host buffer for results
86 pcl::Indices sizes, data, cpu_tmp;
87 // Process all points in the cloud
88 for (std::size_t i = 0; i < host_cloud_->size(); ++i) {
89 // if we already processed this point continue with the next one
90 if (processed[i])
91 continue;
92 // now we will process this point
93 processed[i] = true;
94
95 // Create the query queue on the device, point based not indices
96 pcl::gpu::Octree::Queries queries_device;
97 // Create the query queue on the host
99
100 // Buffer in a new PointXYZ type
101 PointXYZ p;
102 pcl::copyPoint((*host_cloud_)[i], p);
103
104 // Push the starting point in the vector
105 queries_host.push_back(p);
106 // Clear vector
107 r.indices.clear();
108 // Push the starting point in
109 r.indices.push_back(i);
110
111 unsigned int found_points = queries_host.size();
112 unsigned int previous_found_points = 0;
113
114 pcl::gpu::NeighborIndices result_device;
115
116 // once the area stop growing, stop also iterating.
117 do {
118 data.clear();
119 // if the number of queries is not high enough implement search on Host here
120 if (queries_host.size() <=
121 10) ///@todo: adjust this to a variable number settable with method
122 {
123 PCL_DEBUG(" CPU: ");
124 for (std::size_t p = 0; p < queries_host.size(); p++) {
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 PCL_DEBUG(" GPU: ");
134 sizes.clear();
135 // Copy buffer
136 queries_device =
137 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 (const auto& idx : data) {
156 if (processed[idx])
157 continue;
158 processed[idx] = true;
159 PointXYZ p;
160 pcl::copyPoint((*host_cloud_)[idx], p);
161 queries_host.push_back(p);
162 found_points++;
163 r.indices.push_back(idx);
164 }
165 PCL_DEBUG(" data.size: %i, foundpoints: %i, previous: %i",
166 data.size(),
167 found_points,
168 previous_found_points);
169 PCL_DEBUG(" new points: %i, next queries size: %i\n",
170 found_points - previous_found_points,
171 queries_host.size());
172 } while (previous_found_points < found_points);
173 // If this queue is satisfactory, add to the clusters
174 if (found_points >= min_pts_per_cluster && found_points <= max_pts_per_cluster) {
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 ()),
178 // r.indices.end ());
179
180 r.header = host_cloud_->header;
181 clusters.push_back(r); // We could avoid a copy by working directly in the vector
182 }
183 }
184}
185
186template <typename PointT>
187void
189 std::vector<pcl::PointIndices>& clusters)
190{
191 /*
192 // Initialize the GPU search tree
193 if (!tree_)
194 {
195 tree_.reset (new pcl::gpu::Octree());
196 ///@todo what do we do if input isn't a PointXYZ cloud?
197 tree_.setCloud(input_);
198 }
199 */
200 if (!tree_->isBuilt()) {
201 tree_->build();
202 }
203 /*
204 if(tree_->cloud_.size() != host_cloud.size ())
205 {
206 PCL_ERROR("[pcl::gpu::EuclideanClusterExtraction] size of host cloud and device
207 cloud don't match!\n"); return;
208 }
209 */
210 // Extract the actual clusters
211 extractEuclideanClusters<PointT>(host_cloud_,
212 tree_,
213 cluster_tolerance_,
214 clusters,
215 min_pts_per_cluster_,
216 max_pts_per_cluster_);
217 PCL_DEBUG("INFO: end of extractEuclideanClusters\n");
218 // Sort the clusters based on their size (largest one first)
219 // std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);
220}
221
222#define PCL_INSTANTIATE_extractEuclideanClusters(T) \
223 template void PCL_EXPORTS pcl::gpu::extractEuclideanClusters<T>( \
224 const typename pcl::PointCloud<T>::Ptr&, \
225 const pcl::gpu::Octree::Ptr&, \
226 float, \
227 std::vector<PointIndices>&, \
228 unsigned int, \
229 unsigned int);
230#define PCL_INSTANTIATE_EuclideanClusterExtraction(T) \
231 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.
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