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);
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;
93 if ((dx*dy) > static_cast<int64_t> (std::numeric_limits<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> (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_));
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 (!pcl_isfinite (input_->points[*it].x) ||
125 !pcl_isfinite (input_->points[*it].y) ||
126 !pcl_isfinite (input_->points[*it].z))
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]));
133 int idx = ijk0 * divb_mul[0] + ijk1 * divb_mul[1];
134 index_vector.push_back (
point_index_idx (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.push_back (std::pair<unsigned int, unsigned int> (index, i));
164 indices.resize (total);
168 for (
unsigned int cp = 0; cp < first_and_last_indices_vector.size (); ++cp)
170 unsigned int first_index = first_and_last_indices_vector[cp].first;
171 unsigned int last_index = first_and_last_indices_vector[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;
175 for (
unsigned int i = first_index + 1; i < last_index; ++i)
177 if (input_->points[index_vector[i].cloud_point_index].z < min_z)
179 min_z = input_->points[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>; 197 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_ std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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).
unsigned int cloud_point_index
Define standard C methods and C++ classes that are common to all methods.
uint32_t width
The point cloud width (if organized as an image-structure).
point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
bool operator<(const point_index_idx &p) const
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).
void applyFilter(PointCloud &output)
Downsample a Point Cloud using a 2D grid approach.