Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
60template <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////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
80template <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<point_index_idx> ());
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
typename FilterIndices< PointT >::PointCloud PointCloud
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.
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
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