42 #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_
43 #define PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_
46 #include <pcl/common/io.h>
47 #include <pcl/filters/grid_minimum.h>
59 template <
typename Po
intT>
void
65 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
71 std::vector<int> indices;
74 applyFilterIndices (indices);
75 pcl::copyPointCloud<PointT> (*input_, indices, output);
79 template <
typename Po
intT>
void
82 indices.resize (indices_->size ());
86 Eigen::Vector4f min_p, max_p;
87 getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
93 if ((dx*dy) >
static_cast<std::int64_t> (std::numeric_limits<std::int32_t>::max ()))
95 PCL_WARN (
"[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName ().c_str ());
99 Eigen::Vector4i min_b, max_b, div_b, divb_mul;
102 min_b[0] =
static_cast<int> (std::floor (min_p[0] * inverse_resolution_));
103 max_b[0] =
static_cast<int> (std::floor (max_p[0] * inverse_resolution_));
104 min_b[1] =
static_cast<int> (std::floor (min_p[1] * inverse_resolution_));
105 max_b[1] =
static_cast<int> (std::floor (max_p[1] * inverse_resolution_));
108 div_b = max_b - min_b + Eigen::Vector4i::Ones ();
112 divb_mul = Eigen::Vector4i (1, div_b[0], 0, 0);
114 std::vector<point_index_idx> index_vector;
115 index_vector.reserve (indices_->size ());
120 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
122 if (!input_->is_dense)
124 if (!std::isfinite ((*input_)[*it].x) ||
125 !std::isfinite ((*input_)[*it].y) ||
126 !std::isfinite ((*input_)[*it].z))
129 int ijk0 =
static_cast<int> (std::floor ((*input_)[*it].x * inverse_resolution_) -
static_cast<float> (min_b[0]));
130 int ijk1 =
static_cast<int> (std::floor ((*input_)[*it].y * inverse_resolution_) -
static_cast<float> (min_b[1]));
133 int idx = ijk0 * divb_mul[0] + ijk1 * divb_mul[1];
134 index_vector.emplace_back(
static_cast<unsigned int> (idx), *it);
139 std::sort (index_vector.begin (), index_vector.end (), std::less<point_index_idx> ());
143 unsigned int total = 0;
144 unsigned int index = 0;
149 std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
152 first_and_last_indices_vector.reserve (index_vector.size ());
153 while (index < index_vector.size ())
155 unsigned int i = index + 1;
156 while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
159 first_and_last_indices_vector.emplace_back(index, i);
164 indices.resize (total);
168 for (
const auto &cp : first_and_last_indices_vector)
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_)[index_vector[first_index].cloud_point_index].z;
175 for (
unsigned int i = first_index + 1; i < last_index; ++i)
177 if ((*input_)[index_vector[i].cloud_point_index].z < min_z)
179 min_z = (*input_)[index_vector[i].cloud_point_index].z;
180 min_index = index_vector[i].cloud_point_index;
184 indices[index] = min_index;
189 oii = indices.size ();
192 indices.resize (oii);
195 #define PCL_INSTANTIATE_GridMinimum(T) template class PCL_EXPORTS pcl::GridMinimum<T>;
void applyFilterIndices(std::vector< int > &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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Define standard C methods and C++ classes that are common to all methods.
point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
bool operator<(const point_index_idx &p) const
unsigned int cloud_point_index