Point Cloud Library (PCL)  1.13.0-dev
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 ///////////////////////////////////////////////////////////////////////////////////////////
50 template <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 ///////////////////////////////////////////////////////////////////////////////////////////
131 template <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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
225 template <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 generic class that computes the centroid of points fed to it.
Definition: centroid.h:1023
void get(PointOutT &point) const
Retrieve the current centroid.
Definition: centroid.hpp:903
void add(const PointT &point)
Add a new point to the centroid computation.
Definition: centroid.hpp:894
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
std::size_t size() const
Definition: point_cloud.h:443
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
Definition: voxel_grid.hpp:226
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
Definition: point_tests.h:125
unsigned int cloud_point_index
Definition: voxel_grid.hpp:217
cloud_point_index_idx()=default
bool operator<(const cloud_point_index_idx &p) const
Definition: voxel_grid.hpp:221
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
Definition: voxel_grid.hpp:220