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_