Point Cloud Library (PCL)  1.7.2
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/common.h>
42 #include <pcl/common/io.h>
43 #include <pcl/filters/voxel_grid.h>
44 
45 ///////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointT> void
48  const std::string &distance_field_name, float min_distance, float max_distance,
49  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
50 {
51  Eigen::Array4f min_p, max_p;
52  min_p.setConstant (FLT_MAX);
53  max_p.setConstant (-FLT_MAX);
54 
55  // Get the fields list and the distance field index
56  std::vector<pcl::PCLPointField> fields;
57  int distance_idx = pcl::getFieldIndex (*cloud, distance_field_name, fields);
58 
59  float distance_value;
60  // If dense, no need to check for NaNs
61  if (cloud->is_dense)
62  {
63  for (size_t i = 0; i < cloud->points.size (); ++i)
64  {
65  // Get the distance value
66  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[i]);
67  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
68 
69  if (limit_negative)
70  {
71  // Use a threshold for cutting out points which inside the interval
72  if ((distance_value < max_distance) && (distance_value > min_distance))
73  continue;
74  }
75  else
76  {
77  // Use a threshold for cutting out points which are too close/far away
78  if ((distance_value > max_distance) || (distance_value < min_distance))
79  continue;
80  }
81  // Create the point structure and get the min/max
82  pcl::Array4fMapConst pt = cloud->points[i].getArray4fMap ();
83  min_p = min_p.min (pt);
84  max_p = max_p.max (pt);
85  }
86  }
87  else
88  {
89  for (size_t i = 0; i < cloud->points.size (); ++i)
90  {
91  // Get the distance value
92  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[i]);
93  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
94 
95  if (limit_negative)
96  {
97  // Use a threshold for cutting out points which inside the interval
98  if ((distance_value < max_distance) && (distance_value > min_distance))
99  continue;
100  }
101  else
102  {
103  // Use a threshold for cutting out points which are too close/far away
104  if ((distance_value > max_distance) || (distance_value < min_distance))
105  continue;
106  }
107 
108  // Check if the point is invalid
109  if (!pcl_isfinite (cloud->points[i].x) ||
110  !pcl_isfinite (cloud->points[i].y) ||
111  !pcl_isfinite (cloud->points[i].z))
112  continue;
113  // Create the point structure and get the min/max
114  pcl::Array4fMapConst pt = cloud->points[i].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 (*cloud, 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 (std::vector<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
143  {
144  // Get the distance value
145  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[*it]);
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->points[*it].getArray4fMap ();
162  min_p = min_p.min (pt);
163  max_p = max_p.max (pt);
164  }
165  }
166  else
167  {
168  for (std::vector<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
169  {
170  // Get the distance value
171  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[*it]);
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 (!pcl_isfinite (cloud->points[*it].x) ||
189  !pcl_isfinite (cloud->points[*it].y) ||
190  !pcl_isfinite (cloud->points[*it].z))
191  continue;
192  // Create the point structure and get the min/max
193  pcl::Array4fMapConst pt = cloud->points[*it].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 (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
208  bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); }
209 };
210 
211 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
212 template <typename PointT> void
214 {
215  // Has the input dataset been set already?
216  if (!input_)
217  {
218  PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
219  output.width = output.height = 0;
220  output.points.clear ();
221  return;
222  }
223 
224  // Copy the header (and thus the frame_id) + allocate enough space for points
225  output.height = 1; // downsampling breaks the organized structure
226  output.is_dense = true; // we filter out invalid points
227 
228  Eigen::Vector4f min_p, max_p;
229  // Get the minimum and maximum dimensions
230  if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
231  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_);
232  else
233  getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
234 
235  // Check that the leaf size is not too small, given the size of the data
236  int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
237  int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
238  int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
239 
240  if ((dx*dy*dz) > static_cast<int64_t>(std::numeric_limits<int32_t>::max()))
241  {
242  PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
243  output = *input_;
244  return;
245  }
246 
247  // Compute the minimum and maximum bounding box values
248  min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
249  max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
250  min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
251  max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
252  min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
253  max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));
254 
255  // Compute the number of divisions needed along all axis
256  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
257  div_b_[3] = 0;
258 
259  // Set up the division multiplier
260  divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
261 
262  int centroid_size = 4;
263  if (downsample_all_data_)
264  centroid_size = boost::mpl::size<FieldList>::value;
265 
266  // ---[ RGB special case
267  std::vector<pcl::PCLPointField> fields;
268  int rgba_index = -1;
269  rgba_index = pcl::getFieldIndex (*input_, "rgb", fields);
270  if (rgba_index == -1)
271  rgba_index = pcl::getFieldIndex (*input_, "rgba", fields);
272  if (rgba_index >= 0)
273  {
274  rgba_index = fields[rgba_index].offset;
275  centroid_size += 3;
276  }
277 
278  std::vector<cloud_point_index_idx> index_vector;
279  index_vector.reserve (indices_->size ());
280 
281  // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
282  if (!filter_field_name_.empty ())
283  {
284  // Get the distance field index
285  std::vector<pcl::PCLPointField> fields;
286  int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_, fields);
287  if (distance_idx == -1)
288  PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
289 
290  // First pass: go over all points and insert them into the index_vector vector
291  // with calculated idx. Points with the same idx value will contribute to the
292  // same point of resulting CloudPoint
293  for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
294  {
295  if (!input_->is_dense)
296  // Check if the point is invalid
297  if (!pcl_isfinite (input_->points[*it].x) ||
298  !pcl_isfinite (input_->points[*it].y) ||
299  !pcl_isfinite (input_->points[*it].z))
300  continue;
301 
302  // Get the distance value
303  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&input_->points[*it]);
304  float distance_value = 0;
305  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
306 
307  if (filter_limit_negative_)
308  {
309  // Use a threshold for cutting out points which inside the interval
310  if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
311  continue;
312  }
313  else
314  {
315  // Use a threshold for cutting out points which are too close/far away
316  if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
317  continue;
318  }
319 
320  int ijk0 = static_cast<int> (floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
321  int ijk1 = static_cast<int> (floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
322  int ijk2 = static_cast<int> (floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
323 
324  // Compute the centroid leaf index
325  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
326  index_vector.push_back (cloud_point_index_idx (static_cast<unsigned int> (idx), *it));
327  }
328  }
329  // No distance filtering, process all data
330  else
331  {
332  // First pass: go over all points and insert them into the index_vector vector
333  // with calculated idx. Points with the same idx value will contribute to the
334  // same point of resulting CloudPoint
335  for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
336  {
337  if (!input_->is_dense)
338  // Check if the point is invalid
339  if (!pcl_isfinite (input_->points[*it].x) ||
340  !pcl_isfinite (input_->points[*it].y) ||
341  !pcl_isfinite (input_->points[*it].z))
342  continue;
343 
344  int ijk0 = static_cast<int> (floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
345  int ijk1 = static_cast<int> (floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
346  int ijk2 = static_cast<int> (floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
347 
348  // Compute the centroid leaf index
349  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
350  index_vector.push_back (cloud_point_index_idx (static_cast<unsigned int> (idx), *it));
351  }
352  }
353 
354  // Second pass: sort the index_vector vector using value representing target cell as index
355  // in effect all points belonging to the same output cell will be next to each other
356  std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ());
357 
358  // Third pass: count output cells
359  // we need to skip all the same, adjacenent idx values
360  unsigned int total = 0;
361  unsigned int index = 0;
362  // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
363  // index_vector belonging to the voxel which corresponds to the i-th output point,
364  // and of the first point not belonging to.
365  std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
366  // Worst case size
367  first_and_last_indices_vector.reserve (index_vector.size ());
368  while (index < index_vector.size ())
369  {
370  unsigned int i = index + 1;
371  while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
372  ++i;
373  if (i - index >= min_points_per_voxel_)
374  {
375  ++total;
376  first_and_last_indices_vector.push_back (std::pair<unsigned int, unsigned int> (index, i));
377  }
378  index = i;
379  }
380 
381  // Fourth pass: compute centroids, insert them into their final position
382  output.points.resize (total);
383  if (save_leaf_layout_)
384  {
385  try
386  {
387  // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1
388  uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
389  //This is the number of elements that need to be re-initialized to -1
390  uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
391  for (uint32_t i = 0; i < reinit_size; i++)
392  {
393  leaf_layout_[i] = -1;
394  }
395  leaf_layout_.resize (new_layout_size, -1);
396  }
397  catch (std::bad_alloc&)
398  {
399  throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
400  "voxel_grid.hpp", "applyFilter");
401  }
402  catch (std::length_error&)
403  {
404  throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
405  "voxel_grid.hpp", "applyFilter");
406  }
407  }
408 
409  index = 0;
410  Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
411  Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size);
412 
413  for (unsigned int cp = 0; cp < first_and_last_indices_vector.size (); ++cp)
414  {
415  // calculate centroid - sum values from all input points, that have the same idx value in index_vector array
416  unsigned int first_index = first_and_last_indices_vector[cp].first;
417  unsigned int last_index = first_and_last_indices_vector[cp].second;
418  if (!downsample_all_data_)
419  {
420  centroid[0] = input_->points[index_vector[first_index].cloud_point_index].x;
421  centroid[1] = input_->points[index_vector[first_index].cloud_point_index].y;
422  centroid[2] = input_->points[index_vector[first_index].cloud_point_index].z;
423  }
424  else
425  {
426  // ---[ RGB special case
427  if (rgba_index >= 0)
428  {
429  // Fill r/g/b data, assuming that the order is BGRA
430  pcl::RGB rgb;
431  memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[first_index].cloud_point_index]) + rgba_index, sizeof (RGB));
432  centroid[centroid_size-3] = rgb.r;
433  centroid[centroid_size-2] = rgb.g;
434  centroid[centroid_size-1] = rgb.b;
435  }
436  pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[first_index].cloud_point_index], centroid));
437  }
438 
439  for (unsigned int i = first_index + 1; i < last_index; ++i)
440  {
441  if (!downsample_all_data_)
442  {
443  centroid[0] += input_->points[index_vector[i].cloud_point_index].x;
444  centroid[1] += input_->points[index_vector[i].cloud_point_index].y;
445  centroid[2] += input_->points[index_vector[i].cloud_point_index].z;
446  }
447  else
448  {
449  // ---[ RGB special case
450  if (rgba_index >= 0)
451  {
452  // Fill r/g/b data, assuming that the order is BGRA
453  pcl::RGB rgb;
454  memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[i].cloud_point_index]) + rgba_index, sizeof (RGB));
455  temporary[centroid_size-3] = rgb.r;
456  temporary[centroid_size-2] = rgb.g;
457  temporary[centroid_size-1] = rgb.b;
458  }
459  pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[i].cloud_point_index], temporary));
460  centroid += temporary;
461  }
462  }
463 
464  // index is centroid final position in resulting PointCloud
465  if (save_leaf_layout_)
466  leaf_layout_[index_vector[first_index].idx] = index;
467 
468  centroid /= static_cast<float> (last_index - first_index);
469 
470  // store centroid
471  // Do we need to process all the fields?
472  if (!downsample_all_data_)
473  {
474  output.points[index].x = centroid[0];
475  output.points[index].y = centroid[1];
476  output.points[index].z = centroid[2];
477  }
478  else
479  {
480  pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor <PointT> (centroid, output.points[index]));
481  // ---[ RGB special case
482  if (rgba_index >= 0)
483  {
484  // pack r/g/b into rgb
485  float r = centroid[centroid_size-3], g = centroid[centroid_size-2], b = centroid[centroid_size-1];
486  int rgb = (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b);
487  memcpy (reinterpret_cast<char*> (&output.points[index]) + rgba_index, &rgb, sizeof (float));
488  }
489  }
490  ++index;
491  }
492  output.width = static_cast<uint32_t> (output.points.size ());
493 }
494 
495 #define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
496 #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);
497 
498 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_
499 
Helper functor structure for copying data between an Eigen type and a PointT.
Definition: point_cloud.h:103
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
void applyFilter(PointCloud &output)
Downsample a Point Cloud using a voxelized grid approach.
Definition: voxel_grid.hpp:213
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.h:59
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
Helper functor structure for copying data between an Eigen type and a PointT.
Definition: point_cloud.h:70
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:65
bool operator<(const cloud_point_index_idx &p) const
Definition: voxel_grid.hpp:208
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
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:212
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
PointCloud represents the base class in PCL for storing collections of 3D points. ...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
unsigned int cloud_point_index
Definition: voxel_grid.hpp:205
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
Definition: voxel_grid.hpp:207