Point Cloud Library (PCL)  1.14.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/common/point_tests.h> // for isXYZFinite
48 #include <pcl/filters/grid_minimum.h>
49 
51 {
52  unsigned int idx;
53  unsigned int cloud_point_index;
54 
55  point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
56  bool operator < (const point_index_idx &p) const { return (idx < p.idx); }
57 };
58 
59 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
60 template <typename PointT> void
62 {
63  // Has the input dataset been set already?
64  if (!input_)
65  {
66  PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
67  output.width = output.height = 0;
68  output.clear ();
69  return;
70  }
71 
72  Indices indices;
73 
74  output.is_dense = true;
75  applyFilterIndices (indices);
76  pcl::copyPointCloud<PointT> (*input_, indices, output);
77 }
78 
79 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
80 template <typename PointT> void
82 {
83  indices.resize (indices_->size ());
84  int oii = 0;
85 
86  // Get the minimum and maximum dimensions
87  Eigen::Vector4f min_p, max_p;
88  getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
89 
90  // Check that the resolution is not too small, given the size of the data
91  std::int64_t dx = static_cast<std::int64_t> ((max_p[0] - min_p[0]) * inverse_resolution_)+1;
92  std::int64_t dy = static_cast<std::int64_t> ((max_p[1] - min_p[1]) * inverse_resolution_)+1;
93 
94  if ((dx*dy) > static_cast<std::int64_t> (std::numeric_limits<std::int32_t>::max ()))
95  {
96  PCL_WARN ("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName ().c_str ());
97  return;
98  }
99 
100  Eigen::Vector4i min_b, max_b, div_b, divb_mul;
101 
102  // Compute the minimum and maximum bounding box values
103  min_b[0] = static_cast<int> (std::floor (min_p[0] * inverse_resolution_));
104  max_b[0] = static_cast<int> (std::floor (max_p[0] * inverse_resolution_));
105  min_b[1] = static_cast<int> (std::floor (min_p[1] * inverse_resolution_));
106  max_b[1] = static_cast<int> (std::floor (max_p[1] * inverse_resolution_));
107 
108  // Compute the number of divisions needed along all axis
109  div_b = max_b - min_b + Eigen::Vector4i::Ones ();
110  div_b[3] = 0;
111 
112  // Set up the division multiplier
113  divb_mul = Eigen::Vector4i (1, div_b[0], 0, 0);
114 
115  std::vector<point_index_idx> index_vector;
116  index_vector.reserve (indices_->size ());
117 
118  // First pass: go over all points and insert them into the index_vector vector
119  // with calculated idx. Points with the same idx value will contribute to the
120  // same point of resulting CloudPoint
121  for (const auto& index : (*indices_))
122  {
123  if (!input_->is_dense)
124  // Check if the point is invalid
125  if (!isXYZFinite ((*input_)[index]))
126  continue;
127 
128  int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_resolution_) - static_cast<float> (min_b[0]));
129  int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_resolution_) - static_cast<float> (min_b[1]));
130 
131  // Compute the grid cell index
132  int idx = ijk0 * divb_mul[0] + ijk1 * divb_mul[1];
133  index_vector.emplace_back(static_cast<unsigned int> (idx), index);
134  }
135 
136  // Second pass: sort the index_vector vector using value representing target cell as index
137  // in effect all points belonging to the same output cell will be next to each other
138  std::sort (index_vector.begin (), index_vector.end (), std::less<> ());
139 
140  // Third pass: count output cells
141  // we need to skip all the same, adjacenent idx values
142  unsigned int total = 0;
143  unsigned int index = 0;
144 
145  // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
146  // index_vector belonging to the voxel which corresponds to the i-th output point,
147  // and of the first point not belonging to.
148  std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
149 
150  // Worst case size
151  first_and_last_indices_vector.reserve (index_vector.size ());
152  while (index < index_vector.size ())
153  {
154  unsigned int i = index + 1;
155  while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
156  ++i;
157  ++total;
158  first_and_last_indices_vector.emplace_back(index, i);
159  index = i;
160  }
161 
162  // Fourth pass: locate grid minimums
163  indices.resize (total);
164 
165  index = 0;
166 
167  for (const auto &cp : first_and_last_indices_vector)
168  {
169  unsigned int first_index = cp.first;
170  unsigned int last_index = cp.second;
171  unsigned int min_index = index_vector[first_index].cloud_point_index;
172  float min_z = (*input_)[index_vector[first_index].cloud_point_index].z;
173 
174  for (unsigned int i = first_index + 1; i < last_index; ++i)
175  {
176  if ((*input_)[index_vector[i].cloud_point_index].z < min_z)
177  {
178  min_z = (*input_)[index_vector[i].cloud_point_index].z;
179  min_index = index_vector[i].cloud_point_index;
180  }
181  }
182 
183  indices[index] = min_index;
184 
185  ++index;
186  }
187 
188  oii = indices.size ();
189 
190  // Resize the output arrays
191  indices.resize (oii);
192 }
193 
194 #define PCL_INSTANTIATE_GridMinimum(T) template class PCL_EXPORTS pcl::GridMinimum<T>;
195 
196 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_
197 
void applyFilterIndices(Indices &indices)
Filtered results are indexed by an indices array.
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a 2D grid approach.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
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:403
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
Define standard C methods and C++ classes that are common to all methods.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
constexpr bool isXYZFinite(const PointT &) noexcept
Definition: point_tests.h:125
point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
unsigned int idx
bool operator<(const point_index_idx &p) const
unsigned int cloud_point_index