Point Cloud Library (PCL)  1.9.1-dev
grid_minimum.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  * Copyright (c) 2014, RadiantBlue Technologies, Inc.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the copyright holder(s) nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  * $Id$
39  *
40  */
41 
42 #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_
43 #define PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_
44 
45 #include <pcl/common/common.h>
46 #include <pcl/common/io.h>
47 #include <pcl/filters/grid_minimum.h>
48 
50 {
51  unsigned int idx;
52  unsigned int cloud_point_index;
53 
54  point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
55  bool operator < (const point_index_idx &p) const { return (idx < p.idx); }
56 };
57 
58 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
59 template <typename PointT> void
61 {
62  // Has the input dataset been set already?
63  if (!input_)
64  {
65  PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
66  output.width = output.height = 0;
67  output.points.clear ();
68  return;
69  }
70 
71  std::vector<int> indices;
72 
73  output.is_dense = true;
74  applyFilterIndices (indices);
75  pcl::copyPointCloud<PointT> (*input_, indices, output);
76 }
77 
78 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointT> void
81 {
82  indices.resize (indices_->size ());
83  int oii = 0;
84 
85  // Get the minimum and maximum dimensions
86  Eigen::Vector4f min_p, max_p;
87  getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
88 
89  // Check that the resolution is not too small, given the size of the data
90  int64_t dx = static_cast<int64_t> ((max_p[0] - min_p[0]) * inverse_resolution_)+1;
91  int64_t dy = static_cast<int64_t> ((max_p[1] - min_p[1]) * inverse_resolution_)+1;
92 
93  if ((dx*dy) > static_cast<int64_t> (std::numeric_limits<int32_t>::max ()))
94  {
95  PCL_WARN ("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName ().c_str ());
96  return;
97  }
98 
99  Eigen::Vector4i min_b, max_b, div_b, divb_mul;
100 
101  // Compute the minimum and maximum bounding box values
102  min_b[0] = static_cast<int> (floor (min_p[0] * inverse_resolution_));
103  max_b[0] = static_cast<int> (floor (max_p[0] * inverse_resolution_));
104  min_b[1] = static_cast<int> (floor (min_p[1] * inverse_resolution_));
105  max_b[1] = static_cast<int> (floor (max_p[1] * inverse_resolution_));
106 
107  // Compute the number of divisions needed along all axis
108  div_b = max_b - min_b + Eigen::Vector4i::Ones ();
109  div_b[3] = 0;
110 
111  // Set up the division multiplier
112  divb_mul = Eigen::Vector4i (1, div_b[0], 0, 0);
113 
114  std::vector<point_index_idx> index_vector;
115  index_vector.reserve (indices_->size ());
116 
117  // First pass: go over all points and insert them into the index_vector vector
118  // with calculated idx. Points with the same idx value will contribute to the
119  // same point of resulting CloudPoint
120  for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
121  {
122  if (!input_->is_dense)
123  // Check if the point is invalid
124  if (!std::isfinite (input_->points[*it].x) ||
125  !std::isfinite (input_->points[*it].y) ||
126  !std::isfinite (input_->points[*it].z))
127  continue;
128 
129  int ijk0 = static_cast<int> (floor (input_->points[*it].x * inverse_resolution_) - static_cast<float> (min_b[0]));
130  int ijk1 = static_cast<int> (floor (input_->points[*it].y * inverse_resolution_) - static_cast<float> (min_b[1]));
131 
132  // Compute the grid cell index
133  int idx = ijk0 * divb_mul[0] + ijk1 * divb_mul[1];
134  index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
135  }
136 
137  // Second pass: sort the index_vector vector using value representing target cell as index
138  // in effect all points belonging to the same output cell will be next to each other
139  std::sort (index_vector.begin (), index_vector.end (), std::less<point_index_idx> ());
140 
141  // Third pass: count output cells
142  // we need to skip all the same, adjacenent idx values
143  unsigned int total = 0;
144  unsigned int index = 0;
145 
146  // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
147  // index_vector belonging to the voxel which corresponds to the i-th output point,
148  // and of the first point not belonging to.
149  std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
150 
151  // Worst case size
152  first_and_last_indices_vector.reserve (index_vector.size ());
153  while (index < index_vector.size ())
154  {
155  unsigned int i = index + 1;
156  while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
157  ++i;
158  ++total;
159  first_and_last_indices_vector.emplace_back(index, i);
160  index = i;
161  }
162 
163  // Fourth pass: locate grid minimums
164  indices.resize (total);
165 
166  index = 0;
167 
168  for (const auto &cp : first_and_last_indices_vector)
169  {
170  unsigned int first_index = cp.first;
171  unsigned int last_index = cp.second;
172  unsigned int min_index = index_vector[first_index].cloud_point_index;
173  float min_z = input_->points[index_vector[first_index].cloud_point_index].z;
174 
175  for (unsigned int i = first_index + 1; i < last_index; ++i)
176  {
177  if (input_->points[index_vector[i].cloud_point_index].z < min_z)
178  {
179  min_z = input_->points[index_vector[i].cloud_point_index].z;
180  min_index = index_vector[i].cloud_point_index;
181  }
182  }
183 
184  indices[index] = min_index;
185 
186  ++index;
187  }
188 
189  oii = indices.size ();
190 
191  // Resize the output arrays
192  indices.resize (oii);
193 }
194 
195 #define PCL_INSTANTIATE_GridMinimum(T) template class PCL_EXPORTS pcl::GridMinimum<T>;
196 
197 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_
198 
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a 2D grid approach.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
bool operator<(const point_index_idx &p) const
void applyFilterIndices(std::vector< int > &indices)
Filtered results are indexed by an indices array.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
unsigned int cloud_point_index
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
unsigned int idx
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 in any of their floating point fields)...
Definition: point_cloud.h:419