Point Cloud Library (PCL)  1.11.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 <pcl/common/centroid.h>
42 #include <pcl/common/common.h>
43 #include <pcl/common/io.h>
44 #include <pcl/filters/voxel_grid.h>
45 #include <boost/sort/spreadsort/integer_sort.hpp>
46 
47 ///////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointT> void
50  const std::string &distance_field_name, float min_distance, float max_distance,
51  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
52 {
53  Eigen::Array4f min_p, max_p;
54  min_p.setConstant (FLT_MAX);
55  max_p.setConstant (-FLT_MAX);
56 
57  // Get the fields list and the distance field index
58  std::vector<pcl::PCLPointField> fields;
59  int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
60 
61  float distance_value;
62  // If dense, no need to check for NaNs
63  if (cloud->is_dense)
64  {
65  for (const auto& point: *cloud)
66  {
67  // Get the distance value
68  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&point);
69  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
70 
71  if (limit_negative)
72  {
73  // Use a threshold for cutting out points which inside the interval
74  if ((distance_value < max_distance) && (distance_value > min_distance))
75  continue;
76  }
77  else
78  {
79  // Use a threshold for cutting out points which are too close/far away
80  if ((distance_value > max_distance) || (distance_value < min_distance))
81  continue;
82  }
83  // Create the point structure and get the min/max
84  pcl::Array4fMapConst pt = point.getArray4fMap ();
85  min_p = min_p.min (pt);
86  max_p = max_p.max (pt);
87  }
88  }
89  else
90  {
91  for (const auto& point: *cloud)
92  {
93  // Get the distance value
94  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&point);
95  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
96 
97  if (limit_negative)
98  {
99  // Use a threshold for cutting out points which inside the interval
100  if ((distance_value < max_distance) && (distance_value > min_distance))
101  continue;
102  }
103  else
104  {
105  // Use a threshold for cutting out points which are too close/far away
106  if ((distance_value > max_distance) || (distance_value < min_distance))
107  continue;
108  }
109 
110  // Check if the point is invalid
111  if (!isXYZFinite (point))
112  continue;
113  // Create the point structure and get the min/max
114  pcl::Array4fMapConst pt = point.getArray4fMap ();
115  min_p = min_p.min (pt);
116  max_p = max_p.max (pt);
117  }
118  }
119  min_pt = min_p;
120  max_pt = max_p;
121 }
122 
123 ///////////////////////////////////////////////////////////////////////////////////////////
124 template <typename PointT> void
126  const std::vector<int> &indices,
127  const std::string &distance_field_name, float min_distance, float max_distance,
128  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
129 {
130  Eigen::Array4f min_p, max_p;
131  min_p.setConstant (FLT_MAX);
132  max_p.setConstant (-FLT_MAX);
133 
134  // Get the fields list and the distance field index
135  std::vector<pcl::PCLPointField> fields;
136  int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
137 
138  float distance_value;
139  // If dense, no need to check for NaNs
140  if (cloud->is_dense)
141  {
142  for (const int &index : indices)
143  {
144  // Get the distance value
145  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud)[index]);
146  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
147 
148  if (limit_negative)
149  {
150  // Use a threshold for cutting out points which inside the interval
151  if ((distance_value < max_distance) && (distance_value > min_distance))
152  continue;
153  }
154  else
155  {
156  // Use a threshold for cutting out points which are too close/far away
157  if ((distance_value > max_distance) || (distance_value < min_distance))
158  continue;
159  }
160  // Create the point structure and get the min/max
161  pcl::Array4fMapConst pt = (*cloud)[index].getArray4fMap ();
162  min_p = min_p.min (pt);
163  max_p = max_p.max (pt);
164  }
165  }
166  else
167  {
168  for (const int &index : indices)
169  {
170  // Get the distance value
171  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud)[index]);
172  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
173 
174  if (limit_negative)
175  {
176  // Use a threshold for cutting out points which inside the interval
177  if ((distance_value < max_distance) && (distance_value > min_distance))
178  continue;
179  }
180  else
181  {
182  // Use a threshold for cutting out points which are too close/far away
183  if ((distance_value > max_distance) || (distance_value < min_distance))
184  continue;
185  }
186 
187  // Check if the point is invalid
188  if (!std::isfinite ((*cloud)[index].x) ||
189  !std::isfinite ((*cloud)[index].y) ||
190  !std::isfinite ((*cloud)[index].z))
191  continue;
192  // Create the point structure and get the min/max
193  pcl::Array4fMapConst pt = (*cloud)[index].getArray4fMap ();
194  min_p = min_p.min (pt);
195  max_p = max_p.max (pt);
196  }
197  }
198  min_pt = min_p;
199  max_pt = max_p;
200 }
201 
203 {
204  unsigned int idx;
205  unsigned int cloud_point_index;
206 
207  cloud_point_index_idx() = default;
208  cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
209  bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); }
210 };
211 
212 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
213 template <typename PointT> void
215 {
216  // Has the input dataset been set already?
217  if (!input_)
218  {
219  PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
220  output.width = output.height = 0;
221  output.points.clear ();
222  return;
223  }
224 
225  // Copy the header (and thus the frame_id) + allocate enough space for points
226  output.height = 1; // downsampling breaks the organized structure
227  output.is_dense = true; // we filter out invalid points
228 
229  Eigen::Vector4f min_p, max_p;
230  // Get the minimum and maximum dimensions
231  if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
232  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_);
233  else
234  getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
235 
236  // Check that the leaf size is not too small, given the size of the data
237  std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
238  std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
239  std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
240 
241  if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
242  {
243  PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
244  output = *input_;
245  return;
246  }
247 
248  // Compute the minimum and maximum bounding box values
249  min_b_[0] = static_cast<int> (std::floor (min_p[0] * inverse_leaf_size_[0]));
250  max_b_[0] = static_cast<int> (std::floor (max_p[0] * inverse_leaf_size_[0]));
251  min_b_[1] = static_cast<int> (std::floor (min_p[1] * inverse_leaf_size_[1]));
252  max_b_[1] = static_cast<int> (std::floor (max_p[1] * inverse_leaf_size_[1]));
253  min_b_[2] = static_cast<int> (std::floor (min_p[2] * inverse_leaf_size_[2]));
254  max_b_[2] = static_cast<int> (std::floor (max_p[2] * inverse_leaf_size_[2]));
255 
256  // Compute the number of divisions needed along all axis
257  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
258  div_b_[3] = 0;
259 
260  // Set up the division multiplier
261  divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
262 
263  // Storage for mapping leaf and pointcloud indexes
264  std::vector<cloud_point_index_idx> index_vector;
265  index_vector.reserve (indices_->size ());
266 
267  // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
268  if (!filter_field_name_.empty ())
269  {
270  // Get the distance field index
271  std::vector<pcl::PCLPointField> fields;
272  int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
273  if (distance_idx == -1)
274  PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
275 
276  // First pass: go over all points and insert them into the index_vector vector
277  // with calculated idx. Points with the same idx value will contribute to the
278  // same point of resulting CloudPoint
279  for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
280  {
281  if (!input_->is_dense)
282  // Check if the point is invalid
283  if (!std::isfinite ((*input_)[*it].x) ||
284  !std::isfinite ((*input_)[*it].y) ||
285  !std::isfinite ((*input_)[*it].z))
286  continue;
287 
288  // Get the distance value
289  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*input_)[*it]);
290  float distance_value = 0;
291  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
292 
293  if (filter_limit_negative_)
294  {
295  // Use a threshold for cutting out points which inside the interval
296  if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
297  continue;
298  }
299  else
300  {
301  // Use a threshold for cutting out points which are too close/far away
302  if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
303  continue;
304  }
305 
306  int ijk0 = static_cast<int> (std::floor ((*input_)[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
307  int ijk1 = static_cast<int> (std::floor ((*input_)[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
308  int ijk2 = static_cast<int> (std::floor ((*input_)[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
309 
310  // Compute the centroid leaf index
311  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
312  index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
313  }
314  }
315  // No distance filtering, process all data
316  else
317  {
318  // First pass: go over all points and insert them into the index_vector vector
319  // with calculated idx. Points with the same idx value will contribute to the
320  // same point of resulting CloudPoint
321  for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
322  {
323  if (!input_->is_dense)
324  // Check if the point is invalid
325  if (!std::isfinite ((*input_)[*it].x) ||
326  !std::isfinite ((*input_)[*it].y) ||
327  !std::isfinite ((*input_)[*it].z))
328  continue;
329 
330  int ijk0 = static_cast<int> (std::floor ((*input_)[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
331  int ijk1 = static_cast<int> (std::floor ((*input_)[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
332  int ijk2 = static_cast<int> (std::floor ((*input_)[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
333 
334  // Compute the centroid leaf index
335  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
336  index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
337  }
338  }
339 
340  // Second pass: sort the index_vector vector using value representing target cell as index
341  // in effect all points belonging to the same output cell will be next to each other
342  auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; };
343  boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
344 
345  // Third pass: count output cells
346  // we need to skip all the same, adjacent idx values
347  unsigned int total = 0;
348  unsigned int index = 0;
349  // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
350  // index_vector belonging to the voxel which corresponds to the i-th output point,
351  // and of the first point not belonging to.
352  std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
353  // Worst case size
354  first_and_last_indices_vector.reserve (index_vector.size ());
355  while (index < index_vector.size ())
356  {
357  unsigned int i = index + 1;
358  while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
359  ++i;
360  if (i - index >= min_points_per_voxel_)
361  {
362  ++total;
363  first_and_last_indices_vector.emplace_back(index, i);
364  }
365  index = i;
366  }
367 
368  // Fourth pass: compute centroids, insert them into their final position
369  output.points.resize (total);
370  if (save_leaf_layout_)
371  {
372  try
373  {
374  // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1
375  std::uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
376  //This is the number of elements that need to be re-initialized to -1
377  std::uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
378  for (std::uint32_t i = 0; i < reinit_size; i++)
379  {
380  leaf_layout_[i] = -1;
381  }
382  leaf_layout_.resize (new_layout_size, -1);
383  }
384  catch (std::bad_alloc&)
385  {
386  throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
387  "voxel_grid.hpp", "applyFilter");
388  }
389  catch (std::length_error&)
390  {
391  throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
392  "voxel_grid.hpp", "applyFilter");
393  }
394  }
395 
396  index = 0;
397  for (const auto &cp : first_and_last_indices_vector)
398  {
399  // calculate centroid - sum values from all input points, that have the same idx value in index_vector array
400  unsigned int first_index = cp.first;
401  unsigned int last_index = cp.second;
402 
403  // index is centroid final position in resulting PointCloud
404  if (save_leaf_layout_)
405  leaf_layout_[index_vector[first_index].idx] = index;
406 
407  //Limit downsampling to coords
408  if (!downsample_all_data_)
409  {
410  Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
411 
412  for (unsigned int li = first_index; li < last_index; ++li)
413  centroid += (*input_)[index_vector[li].cloud_point_index].getVector4fMap ();
414 
415  centroid /= static_cast<float> (last_index - first_index);
416  output[index].getVector4fMap () = centroid;
417  }
418  else
419  {
420  CentroidPoint<PointT> centroid;
421 
422  // fill in the accumulator with leaf points
423  for (unsigned int li = first_index; li < last_index; ++li)
424  centroid.add ((*input_)[index_vector[li].cloud_point_index]);
425 
426  centroid.get (output[index]);
427  }
428 
429  ++index;
430  }
431  output.width = output.size ();
432 }
433 
434 #define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
435 #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);
436 
437 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_
438 
cloud_point_index_idx::idx
unsigned int idx
Definition: voxel_grid.hpp:204
pcl::uint32_t
std::uint32_t uint32_t
Definition: types.h:58
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
common.h
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
pcl::VoxelGrid::applyFilter
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
Definition: voxel_grid.hpp:214
pcl::isXYZFinite
constexpr bool isXYZFinite(const PointT &) noexcept
Definition: point_tests.h:115
cloud_point_index_idx::cloud_point_index_idx
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
Definition: voxel_grid.hpp:208
pcl::PointCloud< pcl::PointXYZRGBL >
cloud_point_index_idx::cloud_point_index_idx
cloud_point_index_idx()=default
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
pcl::CentroidPoint::get
void get(PointOutT &point) const
Retrieve the current centroid.
Definition: centroid.hpp:867
pcl::CentroidPoint
A generic class that computes the centroid of points fed to it.
Definition: centroid.h:1022
pcl::int64_t
std::int64_t int64_t
Definition: types.h:61
pcl::Array4fMapConst
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
Definition: point_types.hpp:179
pcl::PointCloud::is_dense
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:419
cloud_point_index_idx
Definition: voxel_grid.hpp:202
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:459
pcl::PCLException
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
pcl::getMinMax3D
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:243
pcl::uint8_t
std::uint8_t uint8_t
Definition: types.h:54
cloud_point_index_idx::cloud_point_index
unsigned int cloud_point_index
Definition: voxel_grid.hpp:205
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
cloud_point_index_idx::operator<
bool operator<(const cloud_point_index_idx &p) const
Definition: voxel_grid.hpp:209
pcl::CentroidPoint::add
void add(const PointT &point)
Add a new point to the centroid computation.
Definition: centroid.hpp:858
centroid.h