Point Cloud Library (PCL) 1.15.0
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
214//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
215template <typename PointT> void
217{
218 // Has the input dataset been set already?
219 if (!input_)
220 {
221 PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
222 output.width = output.height = 0;
223 output.clear ();
224 return;
225 }
226
227 // Copy the header (and thus the frame_id) + allocate enough space for points
228 output.height = 1; // downsampling breaks the organized structure
229 output.is_dense = true; // we filter out invalid points
230
231 Eigen::Vector4f min_p, max_p;
232 // Get the minimum and maximum dimensions
233 if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
234 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_);
235 else
236 getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
237
238 // Check that the leaf size is not too small, given the size of the data
239 std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
240 std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
241 std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
242
243 if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
244 {
245 PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName().c_str());
246 output = *input_;
247 return;
248 }
249
250 // Compute the minimum and maximum bounding box values
251 min_b_[0] = static_cast<int> (std::floor (min_p[0] * inverse_leaf_size_[0]));
252 max_b_[0] = static_cast<int> (std::floor (max_p[0] * inverse_leaf_size_[0]));
253 min_b_[1] = static_cast<int> (std::floor (min_p[1] * inverse_leaf_size_[1]));
254 max_b_[1] = static_cast<int> (std::floor (max_p[1] * inverse_leaf_size_[1]));
255 min_b_[2] = static_cast<int> (std::floor (min_p[2] * inverse_leaf_size_[2]));
256 max_b_[2] = static_cast<int> (std::floor (max_p[2] * inverse_leaf_size_[2]));
257
258 // Compute the number of divisions needed along all axis
259 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
260 div_b_[3] = 0;
261
262 // Set up the division multiplier
263 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
264
265 // Storage for mapping leaf and pointcloud indexes
266 std::vector<internal::cloud_point_index_idx> index_vector;
267 index_vector.reserve (indices_->size ());
268
269 // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
270 if (!filter_field_name_.empty ())
271 {
272 // Get the distance field index
273 std::vector<pcl::PCLPointField> fields;
274 int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
275 if (distance_idx == -1) {
276 PCL_ERROR ("[pcl::%s::applyFilter] Invalid filter field name (%s).\n", getClassName ().c_str (), filter_field_name_.c_str());
277 return;
278 }
279 const auto field_offset = fields[distance_idx].offset;
280
281 // First pass: go over all points and insert them into the index_vector vector
282 // with calculated idx. Points with the same idx value will contribute to the
283 // same point of resulting CloudPoint
284 for (const auto& index : (*indices_))
285 {
286 if (!input_->is_dense)
287 // Check if the point is invalid
288 if (!isXYZFinite ((*input_)[index]))
289 continue;
290
291 // Get the distance value
292 const auto* pt_data = reinterpret_cast<const std::uint8_t*> (&(*input_)[index]);
293 float distance_value = 0;
294 memcpy (&distance_value, pt_data + field_offset, sizeof (float));
295
296 if (filter_limit_negative_)
297 {
298 // Use a threshold for cutting out points which inside the interval
299 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
300 continue;
301 }
302 else
303 {
304 // Use a threshold for cutting out points which are too close/far away
305 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
306 continue;
307 }
308
309 int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
310 int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
311 int ijk2 = static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
312
313 // Compute the centroid leaf index
314 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
315 index_vector.emplace_back(static_cast<unsigned int> (idx), index);
316 }
317 }
318 // No distance filtering, process all data
319 else
320 {
321 // First pass: go over all points and insert them into the index_vector vector
322 // with calculated idx. Points with the same idx value will contribute to the
323 // same point of resulting CloudPoint
324 for (const auto& index : (*indices_))
325 {
326 if (!input_->is_dense)
327 // Check if the point is invalid
328 if (!isXYZFinite ((*input_)[index]))
329 continue;
330
331 int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
332 int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
333 int ijk2 = static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
334
335 // Compute the centroid leaf index
336 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
337 index_vector.emplace_back(static_cast<unsigned int> (idx), index);
338 }
339 }
340
341 // Second pass: sort the index_vector vector using value representing target cell as index
342 // in effect all points belonging to the same output cell will be next to each other
343 auto rightshift_func = [](const internal::cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; };
344 boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
345
346 // Third pass: count output cells
347 // we need to skip all the same, adjacent idx values
348 unsigned int total = 0;
349 unsigned int index = 0;
350 // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
351 // index_vector belonging to the voxel which corresponds to the i-th output point,
352 // and of the first point not belonging to.
353 std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
354 // Worst case size
355 first_and_last_indices_vector.reserve (index_vector.size ());
356 while (index < index_vector.size ())
357 {
358 unsigned int i = index + 1;
359 while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
360 ++i;
361 if (i - index >= min_points_per_voxel_)
362 {
363 ++total;
364 first_and_last_indices_vector.emplace_back(index, i);
365 }
366 index = i;
367 }
368
369 // Fourth pass: compute centroids, insert them into their final position
370 output.resize (total);
371 if (save_leaf_layout_)
372 {
373 try
374 {
375 // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1
376 std::uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
377 //This is the number of elements that need to be re-initialized to -1
378 std::uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
379 for (std::uint32_t i = 0; i < reinit_size; i++)
380 {
381 leaf_layout_[i] = -1;
382 }
383 leaf_layout_.resize (new_layout_size, -1);
384 }
385 catch (std::bad_alloc&)
386 {
387 throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
388 "voxel_grid.hpp", "applyFilter");
389 }
390 catch (std::length_error&)
391 {
392 throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
393 "voxel_grid.hpp", "applyFilter");
394 }
395 }
396
397 index = 0;
398 for (const auto &cp : first_and_last_indices_vector)
399 {
400 // calculate centroid - sum values from all input points, that have the same idx value in index_vector array
401 unsigned int first_index = cp.first;
402 unsigned int last_index = cp.second;
403
404 // index is centroid final position in resulting PointCloud
405 if (save_leaf_layout_)
406 leaf_layout_[index_vector[first_index].idx] = index;
407
408 //Limit downsampling to coords
409 if (!downsample_all_data_)
410 {
411 Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
412
413 for (unsigned int li = first_index; li < last_index; ++li)
414 centroid += (*input_)[index_vector[li].cloud_point_index].getVector4fMap ();
415
416 centroid /= static_cast<float> (last_index - first_index);
417 output[index].getVector4fMap () = centroid;
418 }
419 else
420 {
421 CentroidPoint<PointT> centroid;
422
423 // fill in the accumulator with leaf points
424 for (unsigned int li = first_index; li < last_index; ++li)
425 centroid.add ((*input_)[index_vector[li].cloud_point_index]);
426
427 centroid.get (output[index]);
428 }
429
430 ++index;
431 }
432 output.width = output.size ();
433}
434
435#define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
436#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);
437
438#endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_
439
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:67
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:217
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
Used internally in voxel grid classes.
Definition voxel_grid.h:875