219 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().
c_str ());
231 if (!filter_field_name_.empty ())
232 getMinMax3D<PointT> (input_, *indices_, filter_field_name_,
static_cast<float> (filter_limit_min_),
static_cast<float> (filter_limit_max_),
min_p,
max_p, filter_limit_negative_);
237 std::int64_t dx =
static_cast<std::int64_t
>((
max_p[0] -
min_p[0]) * inverse_leaf_size_[0])+1;
238 std::int64_t
dy =
static_cast<std::int64_t
>((
max_p[1] -
min_p[1]) * inverse_leaf_size_[1])+1;
239 std::int64_t
dz =
static_cast<std::int64_t
>((
max_p[2] -
min_p[2]) * inverse_leaf_size_[2])+1;
241 if ((dx*
dy*
dz) >
static_cast<std::int64_t
>(std::numeric_limits<std::int32_t>::max()))
243 PCL_WARN(
"[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName().
c_str());
249 min_b_[0] =
static_cast<int> (std::floor (
min_p[0] * inverse_leaf_size_[0]));
250 max_b_[0] =
static_cast<int> (std::floor (
max_p[0] * inverse_leaf_size_[0]));
251 min_b_[1] =
static_cast<int> (std::floor (
min_p[1] * inverse_leaf_size_[1]));
252 max_b_[1] =
static_cast<int> (std::floor (
max_p[1] * inverse_leaf_size_[1]));
253 min_b_[2] =
static_cast<int> (std::floor (
min_p[2] * inverse_leaf_size_[2]));
254 max_b_[2] =
static_cast<int> (std::floor (
max_p[2] * inverse_leaf_size_[2]));
257 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
261 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
268 if (!filter_field_name_.empty ())
271 std::vector<pcl::PCLPointField> fields;
274 PCL_WARN (
"[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().
c_str (),
distance_idx);
279 for (
const auto& index : (*indices_))
281 if (!input_->is_dense)
287 const std::uint8_t*
pt_data =
reinterpret_cast<const std::uint8_t*
> (&(*input_)[index]);
291 if (filter_limit_negative_)
304 int ijk0 =
static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) -
static_cast<float> (min_b_[0]));
305 int ijk1 =
static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) -
static_cast<float> (min_b_[1]));
306 int ijk2 =
static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) -
static_cast<float> (min_b_[2]));
309 int idx =
ijk0 * divb_mul_[0] +
ijk1 * divb_mul_[1] +
ijk2 * divb_mul_[2];
310 index_vector.emplace_back(
static_cast<unsigned int> (idx), index);
319 for (
const auto& index : (*indices_))
321 if (!input_->is_dense)
326 int ijk0 =
static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) -
static_cast<float> (min_b_[0]));
327 int ijk1 =
static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) -
static_cast<float> (min_b_[1]));
328 int ijk2 =
static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) -
static_cast<float> (min_b_[2]));
331 int idx =
ijk0 * divb_mul_[0] +
ijk1 * divb_mul_[1] +
ijk2 * divb_mul_[2];
332 index_vector.emplace_back(
static_cast<unsigned int> (idx), index);
343 unsigned int total = 0;
344 unsigned int index = 0;
353 unsigned int i = index + 1;
356 if (i - index >= min_points_per_voxel_)
366 if (save_leaf_layout_)
373 std::uint32_t
reinit_size = std::min (
static_cast<unsigned int> (
new_layout_size),
static_cast<unsigned int> (leaf_layout_.size()));
376 leaf_layout_[i] = -1;
380 catch (std::bad_alloc&)
382 throw PCLException(
"VoxelGrid bin size is too low; impossible to allocate memory for layout",
383 "voxel_grid.hpp",
"applyFilter");
385 catch (std::length_error&)
387 throw PCLException(
"VoxelGrid bin size is too low; impossible to allocate memory for layout",
388 "voxel_grid.hpp",
"applyFilter");
400 if (save_leaf_layout_)
404 if (!downsample_all_data_)
406 Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
409 centroid += (*input_)[
index_vector[
li].cloud_point_index].getVector4fMap ();
412 output[index].getVector4fMap () = centroid;
422 centroid.get (
output[index]);
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.