Point Cloud Library (PCL)  1.8.1-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 
46 ///////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT> void
49  const std::string &distance_field_name, float min_distance, float max_distance,
50  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
51 {
52  Eigen::Array4f min_p, max_p;
53  min_p.setConstant (FLT_MAX);
54  max_p.setConstant (-FLT_MAX);
55 
56  // Get the fields list and the distance field index
57  std::vector<pcl::PCLPointField> fields;
58  int distance_idx = pcl::getFieldIndex (*cloud, distance_field_name, fields);
59 
60  float distance_value;
61  // If dense, no need to check for NaNs
62  if (cloud->is_dense)
63  {
64  for (size_t i = 0; i < cloud->points.size (); ++i)
65  {
66  // Get the distance value
67  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[i]);
68  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
69 
70  if (limit_negative)
71  {
72  // Use a threshold for cutting out points which inside the interval
73  if ((distance_value < max_distance) && (distance_value > min_distance))
74  continue;
75  }
76  else
77  {
78  // Use a threshold for cutting out points which are too close/far away
79  if ((distance_value > max_distance) || (distance_value < min_distance))
80  continue;
81  }
82  // Create the point structure and get the min/max
83  pcl::Array4fMapConst pt = cloud->points[i].getArray4fMap ();
84  min_p = min_p.min (pt);
85  max_p = max_p.max (pt);
86  }
87  }
88  else
89  {
90  for (size_t i = 0; i < cloud->points.size (); ++i)
91  {
92  // Get the distance value
93  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[i]);
94  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
95 
96  if (limit_negative)
97  {
98  // Use a threshold for cutting out points which inside the interval
99  if ((distance_value < max_distance) && (distance_value > min_distance))
100  continue;
101  }
102  else
103  {
104  // Use a threshold for cutting out points which are too close/far away
105  if ((distance_value > max_distance) || (distance_value < min_distance))
106  continue;
107  }
108 
109  // Check if the point is invalid
110  if (!pcl_isfinite (cloud->points[i].x) ||
111  !pcl_isfinite (cloud->points[i].y) ||
112  !pcl_isfinite (cloud->points[i].z))
113  continue;
114  // Create the point structure and get the min/max
115  pcl::Array4fMapConst pt = cloud->points[i].getArray4fMap ();
116  min_p = min_p.min (pt);
117  max_p = max_p.max (pt);
118  }
119  }
120  min_pt = min_p;
121  max_pt = max_p;
122 }
123 
124 ///////////////////////////////////////////////////////////////////////////////////////////
125 template <typename PointT> void
127  const std::vector<int> &indices,
128  const std::string &distance_field_name, float min_distance, float max_distance,
129  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
130 {
131  Eigen::Array4f min_p, max_p;
132  min_p.setConstant (FLT_MAX);
133  max_p.setConstant (-FLT_MAX);
134 
135  // Get the fields list and the distance field index
136  std::vector<pcl::PCLPointField> fields;
137  int distance_idx = pcl::getFieldIndex (*cloud, distance_field_name, fields);
138 
139  float distance_value;
140  // If dense, no need to check for NaNs
141  if (cloud->is_dense)
142  {
143  for (std::vector<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
144  {
145  // Get the distance value
146  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[*it]);
147  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
148 
149  if (limit_negative)
150  {
151  // Use a threshold for cutting out points which inside the interval
152  if ((distance_value < max_distance) && (distance_value > min_distance))
153  continue;
154  }
155  else
156  {
157  // Use a threshold for cutting out points which are too close/far away
158  if ((distance_value > max_distance) || (distance_value < min_distance))
159  continue;
160  }
161  // Create the point structure and get the min/max
162  pcl::Array4fMapConst pt = cloud->points[*it].getArray4fMap ();
163  min_p = min_p.min (pt);
164  max_p = max_p.max (pt);
165  }
166  }
167  else
168  {
169  for (std::vector<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
170  {
171  // Get the distance value
172  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[*it]);
173  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
174 
175  if (limit_negative)
176  {
177  // Use a threshold for cutting out points which inside the interval
178  if ((distance_value < max_distance) && (distance_value > min_distance))
179  continue;
180  }
181  else
182  {
183  // Use a threshold for cutting out points which are too close/far away
184  if ((distance_value > max_distance) || (distance_value < min_distance))
185  continue;
186  }
187 
188  // Check if the point is invalid
189  if (!pcl_isfinite (cloud->points[*it].x) ||
190  !pcl_isfinite (cloud->points[*it].y) ||
191  !pcl_isfinite (cloud->points[*it].z))
192  continue;
193  // Create the point structure and get the min/max
194  pcl::Array4fMapConst pt = cloud->points[*it].getArray4fMap ();
195  min_p = min_p.min (pt);
196  max_p = max_p.max (pt);
197  }
198  }
199  min_pt = min_p;
200  max_pt = max_p;
201 }
202 
204 {
205  unsigned int idx;
206  unsigned int cloud_point_index;
207 
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  int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
238  int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
239  int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
240 
241  if ((dx*dy*dz) > static_cast<int64_t>(std::numeric_limits<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> (floor (min_p[0] * inverse_leaf_size_[0]));
250  max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
251  min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
252  max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
253  min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
254  max_b_[2] = static_cast<int> (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 (*input_, 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 (!pcl_isfinite (input_->points[*it].x) ||
284  !pcl_isfinite (input_->points[*it].y) ||
285  !pcl_isfinite (input_->points[*it].z))
286  continue;
287 
288  // Get the distance value
289  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&input_->points[*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> (floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
307  int ijk1 = static_cast<int> (floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
308  int ijk2 = static_cast<int> (floor (input_->points[*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.push_back (cloud_point_index_idx (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 (!pcl_isfinite (input_->points[*it].x) ||
326  !pcl_isfinite (input_->points[*it].y) ||
327  !pcl_isfinite (input_->points[*it].z))
328  continue;
329 
330  int ijk0 = static_cast<int> (floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
331  int ijk1 = static_cast<int> (floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
332  int ijk2 = static_cast<int> (floor (input_->points[*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.push_back (cloud_point_index_idx (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  std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ());
343 
344  // Third pass: count output cells
345  // we need to skip all the same, adjacenent idx values
346  unsigned int total = 0;
347  unsigned int index = 0;
348  // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
349  // index_vector belonging to the voxel which corresponds to the i-th output point,
350  // and of the first point not belonging to.
351  std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
352  // Worst case size
353  first_and_last_indices_vector.reserve (index_vector.size ());
354  while (index < index_vector.size ())
355  {
356  unsigned int i = index + 1;
357  while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
358  ++i;
359  if (i - index >= min_points_per_voxel_)
360  {
361  ++total;
362  first_and_last_indices_vector.push_back (std::pair<unsigned int, unsigned int> (index, i));
363  }
364  index = i;
365  }
366 
367  // Fourth pass: compute centroids, insert them into their final position
368  output.points.resize (total);
369  if (save_leaf_layout_)
370  {
371  try
372  {
373  // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1
374  uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
375  //This is the number of elements that need to be re-initialized to -1
376  uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
377  for (uint32_t i = 0; i < reinit_size; i++)
378  {
379  leaf_layout_[i] = -1;
380  }
381  leaf_layout_.resize (new_layout_size, -1);
382  }
383  catch (std::bad_alloc&)
384  {
385  throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
386  "voxel_grid.hpp", "applyFilter");
387  }
388  catch (std::length_error&)
389  {
390  throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
391  "voxel_grid.hpp", "applyFilter");
392  }
393  }
394 
395  index = 0;
396  for (unsigned int cp = 0; cp < first_and_last_indices_vector.size (); ++cp)
397  {
398  // calculate centroid - sum values from all input points, that have the same idx value in index_vector array
399  unsigned int first_index = first_and_last_indices_vector[cp].first;
400  unsigned int last_index = first_and_last_indices_vector[cp].second;
401 
402  // index is centroid final position in resulting PointCloud
403  if (save_leaf_layout_)
404  leaf_layout_[index_vector[first_index].idx] = index;
405 
406  //Limit downsampling to coords
407  if (!downsample_all_data_)
408  {
409  Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
410 
411  for (unsigned int li = first_index; li < last_index; ++li)
412  centroid += input_->points[index_vector[li].cloud_point_index].getVector4fMap ();
413 
414  centroid /= static_cast<float> (last_index - first_index);
415  output.points[index].getVector4fMap () = centroid;
416  }
417  else
418  {
419  CentroidPoint<PointT> centroid;
420 
421  // fill in the accumulator with leaf points
422  for (unsigned int li = first_index; li < last_index; ++li)
423  centroid.add (input_->points[index_vector[li].cloud_point_index]);
424 
425  centroid.get (output.points[index]);
426  }
427 
428  ++index;
429  }
430  output.width = static_cast<uint32_t> (output.points.size ());
431 }
432 
433 #define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
434 #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);
435 
436 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_
437 
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:214
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
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
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
bool operator<(const cloud_point_index_idx &p) const
Definition: voxel_grid.hpp:209
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void add(const PointT &point)
Add a new point to the centroid computation.
Definition: centroid.h:1052
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:228
unsigned int cloud_point_index
Definition: voxel_grid.hpp:206
void get(PointOutT &point) const
Retrieve the current centroid.
Definition: centroid.h:1069
A generic class that computes the centroid of points fed to it.
Definition: centroid.h:1037
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
Definition: voxel_grid.hpp:208
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415