Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
voxel_grid.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the copyright holder(s) nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $Id$
35 *
36 */
37
38#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_H_
39#define PCL_FILTERS_IMPL_VOXEL_GRID_H_
40
41#include <limits>
42
43#include <pcl/common/centroid.h>
44#include <pcl/common/common.h>
45#include <pcl/common/io.h>
46#include <pcl/filters/voxel_grid.h>
47#include <boost/sort/spreadsort/integer_sort.hpp>
48
49///////////////////////////////////////////////////////////////////////////////////////////
50template <typename PointT> void
52 const std::string &distance_field_name, float min_distance, float max_distance,
53 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
54{
55 Eigen::Array4f min_p, max_p;
56 min_p.setConstant (std::numeric_limits<float>::max());
57 max_p.setConstant (std::numeric_limits<float>::lowest());
58
59 // Get the fields list and the distance field index
60 std::vector<pcl::PCLPointField> fields;
61 int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
62 if (distance_idx < 0 || fields.empty()) {
63 PCL_ERROR ("[pcl::getMinMax3D] Could not find field with name '%s'!\n", distance_field_name.c_str());
64 return;
65 }
66 const auto field_offset = fields[distance_idx].offset;
67
68 float distance_value;
69 // If dense, no need to check for NaNs
70 if (cloud->is_dense)
71 {
72 for (const auto& point: *cloud)
73 {
74 // Get the distance value
75 const auto* pt_data = reinterpret_cast<const std::uint8_t*> (&point);
76 memcpy (&distance_value, pt_data + field_offset, sizeof (float));
77
78 if (limit_negative)
79 {
80 // Use a threshold for cutting out points which inside the interval
81 if ((distance_value < max_distance) && (distance_value > min_distance))
82 continue;
83 }
84 else
85 {
86 // Use a threshold for cutting out points which are too close/far away
87 if ((distance_value > max_distance) || (distance_value < min_distance))
88 continue;
89 }
90 // Create the point structure and get the min/max
91 pcl::Array4fMapConst pt = point.getArray4fMap ();
92 min_p = min_p.min (pt);
93 max_p = max_p.max (pt);
94 }
95 }
96 else
97 {
98 for (const auto& point: *cloud)
99 {
100 // Get the distance value
101 const auto* pt_data = reinterpret_cast<const std::uint8_t*> (&point);
102 memcpy (&distance_value, pt_data + field_offset, sizeof (float));
103
104 if (limit_negative)
105 {
106 // Use a threshold for cutting out points which inside the interval
107 if ((distance_value < max_distance) && (distance_value > min_distance))
108 continue;
109 }
110 else
111 {
112 // Use a threshold for cutting out points which are too close/far away
113 if ((distance_value > max_distance) || (distance_value < min_distance))
114 continue;
115 }
116
117 // Check if the point is invalid
118 if (!isXYZFinite (point))
119 continue;
120 // Create the point structure and get the min/max
121 pcl::Array4fMapConst pt = point.getArray4fMap ();
122 min_p = min_p.min (pt);
123 max_p = max_p.max (pt);
124 }
125 }
126 min_pt = min_p;
127 max_pt = max_p;
128}
129
130///////////////////////////////////////////////////////////////////////////////////////////
131template <typename PointT> void
133 const Indices &indices,
134 const std::string &distance_field_name, float min_distance, float max_distance,
135 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
136{
137 Eigen::Array4f min_p, max_p;
138 min_p.setConstant (std::numeric_limits<float>::max());
139 max_p.setConstant (std::numeric_limits<float>::lowest());
140
141 // Get the fields list and the distance field index
142 std::vector<pcl::PCLPointField> fields;
143 int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
144 if (distance_idx < 0 || fields.empty()) {
145 PCL_ERROR ("[pcl::getMinMax3D] Could not find field with name '%s'!\n", distance_field_name.c_str());
146 return;
147 }
148 const auto field_offset = fields[distance_idx].offset;
149
150 float distance_value;
151 // If dense, no need to check for NaNs
152 if (cloud->is_dense)
153 {
154 for (const auto &index : indices)
155 {
156 // Get the distance value
157 const auto* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud)[index]);
158 memcpy (&distance_value, pt_data + field_offset, sizeof (float));
159
160 if (limit_negative)
161 {
162 // Use a threshold for cutting out points which inside the interval
163 if ((distance_value < max_distance) && (distance_value > min_distance))
164 continue;
165 }
166 else
167 {
168 // Use a threshold for cutting out points which are too close/far away
169 if ((distance_value > max_distance) || (distance_value < min_distance))
170 continue;
171 }
172 // Create the point structure and get the min/max
173 pcl::Array4fMapConst pt = (*cloud)[index].getArray4fMap ();
174 min_p = min_p.min (pt);
175 max_p = max_p.max (pt);
176 }
177 }
178 else
179 {
180 for (const auto &index : indices)
181 {
182 // Get the distance value
183 const auto* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud)[index]);
184 memcpy (&distance_value, pt_data + field_offset, sizeof (float));
185
186 if (limit_negative)
187 {
188 // Use a threshold for cutting out points which inside the interval
189 if ((distance_value < max_distance) && (distance_value > min_distance))
190 continue;
191 }
192 else
193 {
194 // Use a threshold for cutting out points which are too close/far away
195 if ((distance_value > max_distance) || (distance_value < min_distance))
196 continue;
197 }
198
199 // Check if the point is invalid
200 if (!std::isfinite ((*cloud)[index].x) ||
201 !std::isfinite ((*cloud)[index].y) ||
202 !std::isfinite ((*cloud)[index].z))
203 continue;
204 // Create the point structure and get the min/max
205 pcl::Array4fMapConst pt = (*cloud)[index].getArray4fMap ();
206 min_p = min_p.min (pt);
207 max_p = max_p.max (pt);
208 }
209 }
210 min_pt = min_p;
211 max_pt = max_p;
212}
213
215{
216 unsigned int idx;
217 unsigned int cloud_point_index;
218
220 cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
221 bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); }
222};
223
224//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
225template <typename PointT> void
227{
228 // Has the input dataset been set already?
229 if (!input_)
230 {
231 PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
232 output.width = output.height = 0;
233 output.clear ();
234 return;
235 }
236
237 // Copy the header (and thus the frame_id) + allocate enough space for points
238 output.height = 1; // downsampling breaks the organized structure
239 output.is_dense = true; // we filter out invalid points
240
241 Eigen::Vector4f min_p, max_p;
242 // Get the minimum and maximum dimensions
243 if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
244 getMinMax3D<PointT> (input_, *indices_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
245 else
246 getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
247
248 // Check that the leaf size is not too small, given the size of the data
249 std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
250 std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
251 std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
252
253 if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
254 {
255 PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName().c_str());
256 output = *input_;
257 return;
258 }
259
260 // Compute the minimum and maximum bounding box values
261 min_b_[0] = static_cast<int> (std::floor (min_p[0] * inverse_leaf_size_[0]));
262 max_b_[0] = static_cast<int> (std::floor (max_p[0] * inverse_leaf_size_[0]));
263 min_b_[1] = static_cast<int> (std::floor (min_p[1] * inverse_leaf_size_[1]));
264 max_b_[1] = static_cast<int> (std::floor (max_p[1] * inverse_leaf_size_[1]));
265 min_b_[2] = static_cast<int> (std::floor (min_p[2] * inverse_leaf_size_[2]));
266 max_b_[2] = static_cast<int> (std::floor (max_p[2] * inverse_leaf_size_[2]));
267
268 // Compute the number of divisions needed along all axis
269 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
270 div_b_[3] = 0;
271
272 // Set up the division multiplier
273 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
274
275 // Storage for mapping leaf and pointcloud indexes
276 std::vector<cloud_point_index_idx> index_vector;
277 index_vector.reserve (indices_->size ());
278
279 // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
280 if (!filter_field_name_.empty ())
281 {
282 // Get the distance field index
283 std::vector<pcl::PCLPointField> fields;
284 int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
285 if (distance_idx == -1) {
286 PCL_ERROR ("[pcl::%s::applyFilter] Invalid filter field name (%s).\n", getClassName ().c_str (), filter_field_name_.c_str());
287 return;
288 }
289 const auto field_offset = fields[distance_idx].offset;
290
291 // First pass: go over all points and insert them into the index_vector vector
292 // with calculated idx. Points with the same idx value will contribute to the
293 // same point of resulting CloudPoint
294 for (const auto& index : (*indices_))
295 {
296 if (!input_->is_dense)
297 // Check if the point is invalid
298 if (!isXYZFinite ((*input_)[index]))
299 continue;
300
301 // Get the distance value
302 const auto* pt_data = reinterpret_cast<const std::uint8_t*> (&(*input_)[index]);
303 float distance_value = 0;
304 memcpy (&distance_value, pt_data + field_offset, sizeof (float));
305
306 if (filter_limit_negative_)
307 {
308 // Use a threshold for cutting out points which inside the interval
309 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
310 continue;
311 }
312 else
313 {
314 // Use a threshold for cutting out points which are too close/far away
315 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
316 continue;
317 }
318
319 int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
320 int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
321 int ijk2 = static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
322
323 // Compute the centroid leaf index
324 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
325 index_vector.emplace_back(static_cast<unsigned int> (idx), index);
326 }
327 }
328 // No distance filtering, process all data
329 else
330 {
331 // First pass: go over all points and insert them into the index_vector vector
332 // with calculated idx. Points with the same idx value will contribute to the
333 // same point of resulting CloudPoint
334 for (const auto& index : (*indices_))
335 {
336 if (!input_->is_dense)
337 // Check if the point is invalid
338 if (!isXYZFinite ((*input_)[index]))
339 continue;
340
341 int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
342 int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
343 int ijk2 = static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
344
345 // Compute the centroid leaf index
346 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
347 index_vector.emplace_back(static_cast<unsigned int> (idx), index);
348 }
349 }
350
351 // Second pass: sort the index_vector vector using value representing target cell as index
352 // in effect all points belonging to the same output cell will be next to each other
353 auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; };
354 boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
355
356 // Third pass: count output cells
357 // we need to skip all the same, adjacent idx values
358 unsigned int total = 0;
359 unsigned int index = 0;
360 // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
361 // index_vector belonging to the voxel which corresponds to the i-th output point,
362 // and of the first point not belonging to.
363 std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
364 // Worst case size
365 first_and_last_indices_vector.reserve (index_vector.size ());
366 while (index < index_vector.size ())
367 {
368 unsigned int i = index + 1;
369 while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
370 ++i;
371 if (i - index >= min_points_per_voxel_)
372 {
373 ++total;
374 first_and_last_indices_vector.emplace_back(index, i);
375 }
376 index = i;
377 }
378
379 // Fourth pass: compute centroids, insert them into their final position
380 output.resize (total);
381 if (save_leaf_layout_)
382 {
383 try
384 {
385 // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1
386 std::uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
387 //This is the number of elements that need to be re-initialized to -1
388 std::uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
389 for (std::uint32_t i = 0; i < reinit_size; i++)
390 {
391 leaf_layout_[i] = -1;
392 }
393 leaf_layout_.resize (new_layout_size, -1);
394 }
395 catch (std::bad_alloc&)
396 {
397 throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
398 "voxel_grid.hpp", "applyFilter");
399 }
400 catch (std::length_error&)
401 {
402 throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
403 "voxel_grid.hpp", "applyFilter");
404 }
405 }
406
407 index = 0;
408 for (const auto &cp : first_and_last_indices_vector)
409 {
410 // calculate centroid - sum values from all input points, that have the same idx value in index_vector array
411 unsigned int first_index = cp.first;
412 unsigned int last_index = cp.second;
413
414 // index is centroid final position in resulting PointCloud
415 if (save_leaf_layout_)
416 leaf_layout_[index_vector[first_index].idx] = index;
417
418 //Limit downsampling to coords
419 if (!downsample_all_data_)
420 {
421 Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
422
423 for (unsigned int li = first_index; li < last_index; ++li)
424 centroid += (*input_)[index_vector[li].cloud_point_index].getVector4fMap ();
425
426 centroid /= static_cast<float> (last_index - first_index);
427 output[index].getVector4fMap () = centroid;
428 }
429 else
430 {
431 CentroidPoint<PointT> centroid;
432
433 // fill in the accumulator with leaf points
434 for (unsigned int li = first_index; li < last_index; ++li)
435 centroid.add ((*input_)[index_vector[li].cloud_point_index]);
436
437 centroid.get (output[index]);
438 }
439
440 ++index;
441 }
442 output.width = output.size ();
443}
444
445#define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
446#define PCL_INSTANTIATE_getMinMax3D(T) template PCL_EXPORTS void pcl::getMinMax3D<T> (const pcl::PointCloud<T>::ConstPtr &, const std::string &, float, float, Eigen::Vector4f &, Eigen::Vector4f &, bool);
447
448#endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_
449
Define methods for centroid estimation and covariance matrix calculus.
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition exceptions.h:64
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
shared_ptr< const PointCloud< PointT > > ConstPtr
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
typename Filter< PointT >::PointCloud PointCloud
Definition voxel_grid.h:184
Define standard C methods and C++ classes that are common to all methods.
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
Definition common.hpp:295
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
constexpr bool isXYZFinite(const PointT &) noexcept
unsigned int cloud_point_index
cloud_point_index_idx()=default
bool operator<(const cloud_point_index_idx &p) const
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)