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 (std::size_t i = 0; i < cloud->points.size (); ++i)
66  {
67  // Get the distance value
68  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&cloud->points[i]);
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 = cloud->points[i].getArray4fMap ();
85  min_p = min_p.min (pt);
86  max_p = max_p.max (pt);
87  }
88  }
89  else
90  {
91  for (std::size_t i = 0; i < cloud->points.size (); ++i)
92  {
93  // Get the distance value
94  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&cloud->points[i]);
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 (!std::isfinite (cloud->points[i].x) ||
112  !std::isfinite (cloud->points[i].y) ||
113  !std::isfinite (cloud->points[i].z))
114  continue;
115  // Create the point structure and get the min/max
116  pcl::Array4fMapConst pt = cloud->points[i].getArray4fMap ();
117  min_p = min_p.min (pt);
118  max_p = max_p.max (pt);
119  }
120  }
121  min_pt = min_p;
122  max_pt = max_p;
123 }
124 
125 ///////////////////////////////////////////////////////////////////////////////////////////
126 template <typename PointT> void
128  const std::vector<int> &indices,
129  const std::string &distance_field_name, float min_distance, float max_distance,
130  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
131 {
132  Eigen::Array4f min_p, max_p;
133  min_p.setConstant (FLT_MAX);
134  max_p.setConstant (-FLT_MAX);
135 
136  // Get the fields list and the distance field index
137  std::vector<pcl::PCLPointField> fields;
138  int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
139 
140  float distance_value;
141  // If dense, no need to check for NaNs
142  if (cloud->is_dense)
143  {
144  for (const int &index : indices)
145  {
146  // Get the distance value
147  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&cloud->points[index]);
148  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
149 
150  if (limit_negative)
151  {
152  // Use a threshold for cutting out points which inside the interval
153  if ((distance_value < max_distance) && (distance_value > min_distance))
154  continue;
155  }
156  else
157  {
158  // Use a threshold for cutting out points which are too close/far away
159  if ((distance_value > max_distance) || (distance_value < min_distance))
160  continue;
161  }
162  // Create the point structure and get the min/max
163  pcl::Array4fMapConst pt = cloud->points[index].getArray4fMap ();
164  min_p = min_p.min (pt);
165  max_p = max_p.max (pt);
166  }
167  }
168  else
169  {
170  for (const int &index : indices)
171  {
172  // Get the distance value
173  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&cloud->points[index]);
174  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
175 
176  if (limit_negative)
177  {
178  // Use a threshold for cutting out points which inside the interval
179  if ((distance_value < max_distance) && (distance_value > min_distance))
180  continue;
181  }
182  else
183  {
184  // Use a threshold for cutting out points which are too close/far away
185  if ((distance_value > max_distance) || (distance_value < min_distance))
186  continue;
187  }
188 
189  // Check if the point is invalid
190  if (!std::isfinite (cloud->points[index].x) ||
191  !std::isfinite (cloud->points[index].y) ||
192  !std::isfinite (cloud->points[index].z))
193  continue;
194  // Create the point structure and get the min/max
195  pcl::Array4fMapConst pt = cloud->points[index].getArray4fMap ();
196  min_p = min_p.min (pt);
197  max_p = max_p.max (pt);
198  }
199  }
200  min_pt = min_p;
201  max_pt = max_p;
202 }
203 
205 {
206  unsigned int idx;
207  unsigned int cloud_point_index;
208 
209  cloud_point_index_idx() = default;
210  cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
211  bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); }
212 };
213 
214 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
215 template <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.points.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.", 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<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_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
277 
278  // First pass: go over all points and insert them into the index_vector vector
279  // with calculated idx. Points with the same idx value will contribute to the
280  // same point of resulting CloudPoint
281  for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
282  {
283  if (!input_->is_dense)
284  // Check if the point is invalid
285  if (!std::isfinite (input_->points[*it].x) ||
286  !std::isfinite (input_->points[*it].y) ||
287  !std::isfinite (input_->points[*it].z))
288  continue;
289 
290  // Get the distance value
291  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&input_->points[*it]);
292  float distance_value = 0;
293  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
294 
295  if (filter_limit_negative_)
296  {
297  // Use a threshold for cutting out points which inside the interval
298  if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
299  continue;
300  }
301  else
302  {
303  // Use a threshold for cutting out points which are too close/far away
304  if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
305  continue;
306  }
307 
308  int ijk0 = static_cast<int> (std::floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
309  int ijk1 = static_cast<int> (std::floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
310  int ijk2 = static_cast<int> (std::floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
311 
312  // Compute the centroid leaf index
313  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
314  index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
315  }
316  }
317  // No distance filtering, process all data
318  else
319  {
320  // First pass: go over all points and insert them into the index_vector vector
321  // with calculated idx. Points with the same idx value will contribute to the
322  // same point of resulting CloudPoint
323  for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
324  {
325  if (!input_->is_dense)
326  // Check if the point is invalid
327  if (!std::isfinite (input_->points[*it].x) ||
328  !std::isfinite (input_->points[*it].y) ||
329  !std::isfinite (input_->points[*it].z))
330  continue;
331 
332  int ijk0 = static_cast<int> (std::floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
333  int ijk1 = static_cast<int> (std::floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
334  int ijk2 = static_cast<int> (std::floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
335 
336  // Compute the centroid leaf index
337  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
338  index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
339  }
340  }
341 
342  // Second pass: sort the index_vector vector using value representing target cell as index
343  // in effect all points belonging to the same output cell will be next to each other
344  auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; };
345  boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
346 
347  // Third pass: count output cells
348  // we need to skip all the same, adjacent idx values
349  unsigned int total = 0;
350  unsigned int index = 0;
351  // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
352  // index_vector belonging to the voxel which corresponds to the i-th output point,
353  // and of the first point not belonging to.
354  std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
355  // Worst case size
356  first_and_last_indices_vector.reserve (index_vector.size ());
357  while (index < index_vector.size ())
358  {
359  unsigned int i = index + 1;
360  while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
361  ++i;
362  if (i - index >= min_points_per_voxel_)
363  {
364  ++total;
365  first_and_last_indices_vector.emplace_back(index, i);
366  }
367  index = i;
368  }
369 
370  // Fourth pass: compute centroids, insert them into their final position
371  output.points.resize (total);
372  if (save_leaf_layout_)
373  {
374  try
375  {
376  // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1
377  std::uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
378  //This is the number of elements that need to be re-initialized to -1
379  std::uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
380  for (std::uint32_t i = 0; i < reinit_size; i++)
381  {
382  leaf_layout_[i] = -1;
383  }
384  leaf_layout_.resize (new_layout_size, -1);
385  }
386  catch (std::bad_alloc&)
387  {
388  throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
389  "voxel_grid.hpp", "applyFilter");
390  }
391  catch (std::length_error&)
392  {
393  throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
394  "voxel_grid.hpp", "applyFilter");
395  }
396  }
397 
398  index = 0;
399  for (const auto &cp : first_and_last_indices_vector)
400  {
401  // calculate centroid - sum values from all input points, that have the same idx value in index_vector array
402  unsigned int first_index = cp.first;
403  unsigned int last_index = cp.second;
404 
405  // index is centroid final position in resulting PointCloud
406  if (save_leaf_layout_)
407  leaf_layout_[index_vector[first_index].idx] = index;
408 
409  //Limit downsampling to coords
410  if (!downsample_all_data_)
411  {
412  Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
413 
414  for (unsigned int li = first_index; li < last_index; ++li)
415  centroid += input_->points[index_vector[li].cloud_point_index].getVector4fMap ();
416 
417  centroid /= static_cast<float> (last_index - first_index);
418  output.points[index].getVector4fMap () = centroid;
419  }
420  else
421  {
422  CentroidPoint<PointT> centroid;
423 
424  // fill in the accumulator with leaf points
425  for (unsigned int li = first_index; li < last_index; ++li)
426  centroid.add (input_->points[index_vector[li].cloud_point_index]);
427 
428  centroid.get (output.points[index]);
429  }
430 
431  ++index;
432  }
433  output.width = static_cast<std::uint32_t> (output.points.size ());
434 }
435 
436 #define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
437 #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);
438 
439 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_
440 
cloud_point_index_idx::idx
unsigned int idx
Definition: voxel_grid.hpp:206
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:216
cloud_point_index_idx::cloud_point_index_idx
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
Definition: voxel_grid.hpp:210
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:204
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:242
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:207
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:211
pcl::CentroidPoint::add
void add(const PointT &point)
Add a new point to the centroid computation.
Definition: centroid.hpp:858
centroid.h