voxel_grid.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL18_BACKPORTS_VOXEL_GRID_H
41 #define PCL18_BACKPORTS_VOXEL_GRID_H
42 
43 #ifndef BACKPORT_PCL_VOXEL_GRID
44 
45 #include <pcl/filters/voxel_grid.h>
46 
47 namespace pcl
48 {
49 template <typename PointT>
51 }
52 
53 #else
54 
55 // System has old PCL; backport VoxelGrid from pcl-1.8
56 
57 #include <algorithm>
58 #include <cmath>
59 #include <functional>
60 #include <limits>
61 #include <string>
62 #include <utility>
63 #include <vector>
64 
65 #include <pcl/common/centroid.h>
66 #include <pcl/filters/boost.h>
67 #include <pcl/filters/filter.h>
68 
69 namespace pcl
70 {
83 template <typename PointT>
84 class VoxelGrid18 : public Filter<PointT>
85 {
86 protected:
91 
92  typedef typename Filter<PointT>::PointCloud PointCloud;
93  typedef typename PointCloud::Ptr PointCloudPtr;
94  typedef typename PointCloud::ConstPtr PointCloudConstPtr;
95 
96 public:
99 
101  VoxelGrid18()
102  : leaf_size_(Eigen::Vector4f::Zero())
103  , inverse_leaf_size_(Eigen::Array4f::Zero())
104  , leaf_layout_()
105  , min_b_(Eigen::Vector4i::Zero())
106  , max_b_(Eigen::Vector4i::Zero())
107  , div_b_(Eigen::Vector4i::Zero())
108  , divb_mul_(Eigen::Vector4i::Zero())
109  , filter_limit_min_(-std::numeric_limits<float>::max())
110  , filter_limit_max_(std::numeric_limits<float>::max())
111  , filter_limit_negative_(false)
112  , min_points_per_voxel_(0)
113  {
114  filter_name_ = "VoxelGrid";
115  }
116 
118  virtual ~VoxelGrid18()
119  {
120  }
121 
125  inline void
126  setLeafSize(const Eigen::Vector4f& leaf_size)
127  {
128  leaf_size_ = leaf_size;
129  // Avoid division errors
130  if (leaf_size_[3] == 0)
131  leaf_size_[3] = 1;
132  // Use multiplications instead of divisions
133  inverse_leaf_size_ = Eigen::Array4f::Ones() / leaf_size_.array();
134  }
135 
141  inline void
142  setLeafSize(float lx, float ly, float lz)
143  {
144  leaf_size_[0] = lx;
145  leaf_size_[1] = ly;
146  leaf_size_[2] = lz;
147  // Avoid division errors
148  if (leaf_size_[3] == 0)
149  leaf_size_[3] = 1;
150  // Use multiplications instead of divisions
151  inverse_leaf_size_ = Eigen::Array4f::Ones() / leaf_size_.array();
152  }
153 
155  inline Eigen::Vector3f
156  getLeafSize() const
157  {
158  return (leaf_size_.head<3>());
159  }
160 
164  inline void
165  setMinimumPointsNumberPerVoxel(unsigned int min_points_per_voxel)
166  {
167  min_points_per_voxel_ = min_points_per_voxel;
168  }
169 
172  inline unsigned int
173  getMinimumPointsNumberPerVoxel() const
174  {
175  return min_points_per_voxel_;
176  }
177 
181  inline Eigen::Vector3i
182  getMinBoxCoordinates() const
183  {
184  return (min_b_.head<3>());
185  }
186 
190  inline Eigen::Vector3i
191  getMaxBoxCoordinates() const
192  {
193  return (max_b_.head<3>());
194  }
195 
199  inline Eigen::Vector3i
200  getNrDivisions() const
201  {
202  return (div_b_.head<3>());
203  }
204 
208  inline Eigen::Vector3i
209  getDivisionMultiplier() const
210  {
211  return (divb_mul_.head<3>());
212  }
213 
222  inline int
223  getCentroidIndex(const PointT& p) const
224  {
225  return (leaf_layout_.at((Eigen::Vector4i(static_cast<int>(std::floor(p.x * inverse_leaf_size_[0])),
226  static_cast<int>(std::floor(p.y * inverse_leaf_size_[1])),
227  static_cast<int>(std::floor(p.z * inverse_leaf_size_[2])), 0) -
228  min_b_)
229  .dot(divb_mul_)));
230  }
231 
238  inline std::vector<int>
239  getNeighborCentroidIndices(const PointT& reference_point, const Eigen::MatrixXi& relative_coordinates) const
240  {
241  Eigen::Vector4i ijk(static_cast<int>(std::floor(reference_point.x * inverse_leaf_size_[0])),
242  static_cast<int>(std::floor(reference_point.y * inverse_leaf_size_[1])),
243  static_cast<int>(std::floor(reference_point.z * inverse_leaf_size_[2])), 0);
244  Eigen::Array4i diff2min = min_b_ - ijk;
245  Eigen::Array4i diff2max = max_b_ - ijk;
246  std::vector<int> neighbors(relative_coordinates.cols());
247  for (int ni = 0; ni < relative_coordinates.cols(); ni++)
248  {
249  Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
250  // checking if the specified cell is in the grid
251  if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
252  neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot(divb_mul_))]; // .at() can be omitted
253  else
254  neighbors[ni] = -1; // cell is out of bounds, consider it empty
255  }
256  return (neighbors);
257  }
258 
262  inline std::vector<int>
263  getLeafLayout() const
264  {
265  return (leaf_layout_);
266  }
267 
273  inline Eigen::Vector3i
274  getGridCoordinates(float x, float y, float z) const
275  {
276  return (Eigen::Vector3i(static_cast<int>(std::floor(x * inverse_leaf_size_[0])),
277  static_cast<int>(std::floor(y * inverse_leaf_size_[1])),
278  static_cast<int>(std::floor(z * inverse_leaf_size_[2]))));
279  }
280 
284  inline int
285  getCentroidIndexAt(const Eigen::Vector3i& ijk) const
286  {
287  int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot(divb_mul_);
288  if (idx < 0 || idx >= static_cast<int>(leaf_layout_.size()))
289  // this checks also if leaf_layout_.size () == 0 i.e. everything was computed as needed
290  {
291  return (-1);
292  }
293  return (leaf_layout_[idx]);
294  }
295 
300  inline void
301  setFilterLimits(const double& limit_min, const double& limit_max)
302  {
303  filter_limit_min_ = limit_min;
304  filter_limit_max_ = limit_max;
305  }
306 
311  inline void
312  getFilterLimits(double& limit_min, double& limit_max) const
313  {
314  limit_min = filter_limit_min_;
315  limit_max = filter_limit_max_;
316  }
317 
322  inline void
323  setFilterLimitsNegative(const bool limit_negative)
324  {
325  filter_limit_negative_ = limit_negative;
326  }
327 
331  inline void
332  getFilterLimitsNegative(bool& limit_negative) const
333  {
334  limit_negative = filter_limit_negative_;
335  }
336 
340  inline bool
341  getFilterLimitsNegative() const
342  {
343  return (filter_limit_negative_);
344  }
345 
346 protected:
348  Eigen::Vector4f leaf_size_;
349 
351  Eigen::Array4f inverse_leaf_size_;
352 
354  std::vector<int> leaf_layout_;
355 
357  Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
358 
360  double filter_limit_min_;
361 
363  double filter_limit_max_;
364 
368 
370  unsigned int min_points_per_voxel_;
371 
372  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
373 
377  void
378  applyFilter(PointCloud& output)
379  {
380  // Has the input dataset been set already?
381  if (!input_)
382  {
383  PCL_WARN("[pcl::%s::applyFilter] No input dataset given!\n", getClassName().c_str());
384  output.width = output.height = 0;
385  output.points.clear();
386  return;
387  }
388 
389  // Copy the header (and thus the frame_id) + allocate enough space for points
390  output.height = 1; // downsampling breaks the organized structure
391  output.is_dense = true; // we filter out invalid points
392 
393  Eigen::Vector4f min_p, max_p;
394  getMinMax3D<PointT>(*input_, *indices_, min_p, max_p);
395 
396  // Check that the leaf size is not too small, given the size of the data
397  int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0]) + 1;
398  int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1]) + 1;
399  int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2]) + 1;
400 
401  if ((dx * dy * dz) > static_cast<int64_t>(std::numeric_limits<int32_t>::max()))
402  {
403  PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.",
404  getClassName().c_str());
405  output = *input_;
406  return;
407  }
408 
409  // Compute the minimum and maximum bounding box values
410  min_b_[0] = static_cast<int>(std::floor(min_p[0] * inverse_leaf_size_[0]));
411  max_b_[0] = static_cast<int>(std::floor(max_p[0] * inverse_leaf_size_[0]));
412  min_b_[1] = static_cast<int>(std::floor(min_p[1] * inverse_leaf_size_[1]));
413  max_b_[1] = static_cast<int>(std::floor(max_p[1] * inverse_leaf_size_[1]));
414  min_b_[2] = static_cast<int>(std::floor(min_p[2] * inverse_leaf_size_[2]));
415  max_b_[2] = static_cast<int>(std::floor(max_p[2] * inverse_leaf_size_[2]));
416 
417  // Compute the number of divisions needed along all axis
418  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones();
419  div_b_[3] = 0;
420 
421  // Set up the division multiplier
422  divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0);
423 
424  // Storage for mapping leaf and pointcloud indexes
425  std::vector<cloud_point_index_idx> index_vector;
426  index_vector.reserve(indices_->size());
427 
428  // No distance filtering, process all data
429  // First pass: go over all points and insert them into the index_vector vector
430  // with calculated idx. Points with the same idx value will contribute to the
431  // same point of resulting CloudPoint
432  for (std::vector<int>::const_iterator it = indices_->begin(); it != indices_->end(); ++it)
433  {
434  if (!input_->is_dense)
435  // Check if the point is invalid
436  if (!pcl_isfinite(input_->points[*it].x) ||
437  !pcl_isfinite(input_->points[*it].y) ||
438  !pcl_isfinite(input_->points[*it].z))
439  continue;
440 
441  int ijk0 = static_cast<int>(
442  std::floor(input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float>(min_b_[0]));
443  int ijk1 = static_cast<int>(
444  std::floor(input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float>(min_b_[1]));
445  int ijk2 = static_cast<int>(
446  std::floor(input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float>(min_b_[2]));
447 
448  // Compute the centroid leaf index
449  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
450  index_vector.push_back(cloud_point_index_idx(static_cast<unsigned int>(idx), *it));
451  }
452 
453  // Second pass: sort the index_vector vector using value representing target cell as index
454  // in effect all points belonging to the same output cell will be next to each other
455  std::sort(index_vector.begin(), index_vector.end(), std::less<cloud_point_index_idx>());
456 
457  // Third pass: count output cells
458  // we need to skip all the same, adjacent idx values
459  unsigned int total = 0;
460  unsigned int index = 0;
461  // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
462  // index_vector belonging to the voxel which corresponds to the i-th output point,
463  // and of the first point not belonging to.
464  std::vector<std::pair<unsigned int, unsigned int>> first_and_last_indices_vector;
465  // Worst case size
466  first_and_last_indices_vector.reserve(index_vector.size());
467  while (index < index_vector.size())
468  {
469  unsigned int i = index + 1;
470  while (i < index_vector.size() && index_vector[i].idx == index_vector[index].idx)
471  ++i;
472  if (i - index >= min_points_per_voxel_)
473  {
474  ++total;
475  first_and_last_indices_vector.push_back(std::pair<unsigned int, unsigned int>(index, i));
476  }
477  index = i;
478  }
479 
480  // Fourth pass: compute centroids, insert them into their final position
481  output.points.resize(total);
482 
483  index = 0;
484  for (unsigned int cp = 0; cp < first_and_last_indices_vector.size(); ++cp)
485  {
486  // calculate centroid - sum values from all input points, that have the same idx value in index_vector array
487  unsigned int first_index = first_and_last_indices_vector[cp].first;
488  unsigned int last_index = first_and_last_indices_vector[cp].second;
489 
490  CentroidPoint<PointT> centroid;
491 
492  // fill in the accumulator with leaf points
493  for (unsigned int li = first_index; li < last_index; ++li)
494  centroid.add(input_->points[index_vector[li].cloud_point_index]);
495 
496  centroid.get(output.points[index]);
497 
498  ++index;
499  }
500  output.width = static_cast<uint32_t>(output.points.size());
501  }
502 };
503 } // namespace pcl
504 
505 #endif // BACKPORT_PCL_VOXEL_GRID
506 
507 #endif // PCL18_BACKPORTS_VOXEL_GRID_H
TFSIMD_FORCE_INLINE const tfScalar & y() const
double filter_limit_max_
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
VoxelGrid< PointT > VoxelGrid18
Definition: voxel_grid.h:50
TFSIMD_FORCE_INLINE const tfScalar & x() const
pcl::PointCloud< pcl::PointXYZ > PointCloud
TFSIMD_FORCE_INLINE const tfScalar & z() const
PointCloud::ConstPtr PointCloudConstPtr
double filter_limit_min_
bool filter_limit_negative_
PointCloud::Ptr PointCloudPtr


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:16:29