Point Cloud Library (PCL)  1.7.0
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  while (index < index_vector.size ())
363  {
364  unsigned int i = index + 1;
365  while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
366  ++i;
367  ++total;
368  index = i;
369  }
370 
371  // Fourth pass: compute centroids, insert them into their final position
372  output.points.resize (total);
373  if (save_leaf_layout_)
374  {
375  try
376  {
377  // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1
378  uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
379  //This is the number of elements that need to be re-initialized to -1
380  uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
381  for (uint32_t i = 0; i < reinit_size; i++)
382  {
383  leaf_layout_[i] = -1;
384  }
385  leaf_layout_.resize (new_layout_size, -1);
386  }
387  catch (std::bad_alloc&)
388  {
389  throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
390  "voxel_grid.hpp", "applyFilter");
391  }
392  catch (std::length_error&)
393  {
394  throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
395  "voxel_grid.hpp", "applyFilter");
396  }
397  }
398 
399  index = 0;
400  Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
401  Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size);
402 
403  for (unsigned int cp = 0; cp < index_vector.size ();)
404  {
405  // calculate centroid - sum values from all input points, that have the same idx value in index_vector array
406  if (!downsample_all_data_)
407  {
408  centroid[0] = input_->points[index_vector[cp].cloud_point_index].x;
409  centroid[1] = input_->points[index_vector[cp].cloud_point_index].y;
410  centroid[2] = input_->points[index_vector[cp].cloud_point_index].z;
411  }
412  else
413  {
414  // ---[ RGB special case
415  if (rgba_index >= 0)
416  {
417  // Fill r/g/b data, assuming that the order is BGRA
418  pcl::RGB rgb;
419  memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[cp].cloud_point_index]) + rgba_index, sizeof (RGB));
420  centroid[centroid_size-3] = rgb.r;
421  centroid[centroid_size-2] = rgb.g;
422  centroid[centroid_size-1] = rgb.b;
423  }
424  pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[cp].cloud_point_index], centroid));
425  }
426 
427  unsigned int i = cp + 1;
428  while (i < index_vector.size () && index_vector[i].idx == index_vector[cp].idx)
429  {
430  if (!downsample_all_data_)
431  {
432  centroid[0] += input_->points[index_vector[i].cloud_point_index].x;
433  centroid[1] += input_->points[index_vector[i].cloud_point_index].y;
434  centroid[2] += input_->points[index_vector[i].cloud_point_index].z;
435  }
436  else
437  {
438  // ---[ RGB special case
439  if (rgba_index >= 0)
440  {
441  // Fill r/g/b data, assuming that the order is BGRA
442  pcl::RGB rgb;
443  memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[i].cloud_point_index]) + rgba_index, sizeof (RGB));
444  temporary[centroid_size-3] = rgb.r;
445  temporary[centroid_size-2] = rgb.g;
446  temporary[centroid_size-1] = rgb.b;
447  }
448  pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[i].cloud_point_index], temporary));
449  centroid += temporary;
450  }
451  ++i;
452  }
453 
454  // index is centroid final position in resulting PointCloud
455  if (save_leaf_layout_)
456  leaf_layout_[index_vector[cp].idx] = index;
457 
458  centroid /= static_cast<float> (i - cp);
459 
460  // store centroid
461  // Do we need to process all the fields?
462  if (!downsample_all_data_)
463  {
464  output.points[index].x = centroid[0];
465  output.points[index].y = centroid[1];
466  output.points[index].z = centroid[2];
467  }
468  else
469  {
470  pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor <PointT> (centroid, output.points[index]));
471  // ---[ RGB special case
472  if (rgba_index >= 0)
473  {
474  // pack r/g/b into rgb
475  float r = centroid[centroid_size-3], g = centroid[centroid_size-2], b = centroid[centroid_size-1];
476  int rgb = (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b);
477  memcpy (reinterpret_cast<char*> (&output.points[index]) + rgba_index, &rgb, sizeof (float));
478  }
479  }
480  cp = i;
481  ++index;
482  }
483  output.width = static_cast<uint32_t> (output.points.size ());
484 }
485 
486 #define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
487 #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);
488 
489 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_
490