38 #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_H_
39 #define PCL_FILTERS_IMPL_VOXEL_GRID_H_
45 #include <pcl/common/io.h>
46 #include <pcl/filters/voxel_grid.h>
47 #include <boost/sort/spreadsort/integer_sort.hpp>
50 template <
typename Po
intT>
void
52 const std::string &distance_field_name,
float min_distance,
float max_distance,
53 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative)
55 Eigen::Array4f min_p, max_p;
56 min_p.setConstant (std::numeric_limits<float>::max());
57 max_p.setConstant (std::numeric_limits<float>::lowest());
60 std::vector<pcl::PCLPointField> fields;
61 int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
62 if (distance_idx < 0 || fields.empty()) {
63 PCL_ERROR (
"[pcl::getMinMax3D] Could not find field with name '%s'!\n", distance_field_name.c_str());
66 const auto field_offset = fields[distance_idx].offset;
72 for (
const auto& point: *cloud)
75 const auto* pt_data =
reinterpret_cast<const std::uint8_t*
> (&point);
76 memcpy (&distance_value, pt_data + field_offset,
sizeof (
float));
81 if ((distance_value < max_distance) && (distance_value > min_distance))
87 if ((distance_value > max_distance) || (distance_value < min_distance))
92 min_p = min_p.min (pt);
93 max_p = max_p.max (pt);
98 for (
const auto& point: *cloud)
101 const auto* pt_data =
reinterpret_cast<const std::uint8_t*
> (&point);
102 memcpy (&distance_value, pt_data + field_offset,
sizeof (
float));
107 if ((distance_value < max_distance) && (distance_value > min_distance))
113 if ((distance_value > max_distance) || (distance_value < min_distance))
122 min_p = min_p.min (pt);
123 max_p = max_p.max (pt);
131 template <
typename Po
intT>
void
134 const std::string &distance_field_name,
float min_distance,
float max_distance,
135 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative)
137 Eigen::Array4f min_p, max_p;
138 min_p.setConstant (std::numeric_limits<float>::max());
139 max_p.setConstant (std::numeric_limits<float>::lowest());
142 std::vector<pcl::PCLPointField> fields;
143 int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
144 if (distance_idx < 0 || fields.empty()) {
145 PCL_ERROR (
"[pcl::getMinMax3D] Could not find field with name '%s'!\n", distance_field_name.c_str());
148 const auto field_offset = fields[distance_idx].offset;
150 float distance_value;
154 for (
const auto &index : indices)
157 const auto* pt_data =
reinterpret_cast<const std::uint8_t*
> (&(*cloud)[index]);
158 memcpy (&distance_value, pt_data + field_offset,
sizeof (
float));
163 if ((distance_value < max_distance) && (distance_value > min_distance))
169 if ((distance_value > max_distance) || (distance_value < min_distance))
174 min_p = min_p.min (pt);
175 max_p = max_p.max (pt);
180 for (
const auto &index : indices)
183 const auto* pt_data =
reinterpret_cast<const std::uint8_t*
> (&(*cloud)[index]);
184 memcpy (&distance_value, pt_data + field_offset,
sizeof (
float));
189 if ((distance_value < max_distance) && (distance_value > min_distance))
195 if ((distance_value > max_distance) || (distance_value < min_distance))
200 if (!std::isfinite ((*cloud)[index].x) ||
201 !std::isfinite ((*cloud)[index].y) ||
202 !std::isfinite ((*cloud)[index].z))
206 min_p = min_p.min (pt);
207 max_p = max_p.max (pt);
225 template <
typename Po
intT>
void
231 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
241 Eigen::Vector4f min_p, max_p;
243 if (!filter_field_name_.empty ())
244 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_);
246 getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
249 std::int64_t dx =
static_cast<std::int64_t
>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
250 std::int64_t dy =
static_cast<std::int64_t
>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
251 std::int64_t dz =
static_cast<std::int64_t
>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
253 if ((dx*dy*dz) >
static_cast<std::int64_t
>(std::numeric_limits<std::int32_t>::max()))
255 PCL_WARN(
"[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName().c_str());
261 min_b_[0] =
static_cast<int> (std::floor (min_p[0] * inverse_leaf_size_[0]));
262 max_b_[0] =
static_cast<int> (std::floor (max_p[0] * inverse_leaf_size_[0]));
263 min_b_[1] =
static_cast<int> (std::floor (min_p[1] * inverse_leaf_size_[1]));
264 max_b_[1] =
static_cast<int> (std::floor (max_p[1] * inverse_leaf_size_[1]));
265 min_b_[2] =
static_cast<int> (std::floor (min_p[2] * inverse_leaf_size_[2]));
266 max_b_[2] =
static_cast<int> (std::floor (max_p[2] * inverse_leaf_size_[2]));
269 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
273 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
276 std::vector<cloud_point_index_idx> index_vector;
277 index_vector.reserve (indices_->size ());
280 if (!filter_field_name_.empty ())
283 std::vector<pcl::PCLPointField> fields;
284 int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
285 if (distance_idx == -1) {
286 PCL_ERROR (
"[pcl::%s::applyFilter] Invalid filter field name (%s).\n", getClassName ().c_str (), filter_field_name_.c_str());
289 const auto field_offset = fields[distance_idx].offset;
294 for (
const auto& index : (*indices_))
296 if (!input_->is_dense)
302 const auto* pt_data =
reinterpret_cast<const std::uint8_t*
> (&(*input_)[index]);
303 float distance_value = 0;
304 memcpy (&distance_value, pt_data + field_offset,
sizeof (
float));
306 if (filter_limit_negative_)
309 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
315 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
319 int ijk0 =
static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) -
static_cast<float> (min_b_[0]));
320 int ijk1 =
static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) -
static_cast<float> (min_b_[1]));
321 int ijk2 =
static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) -
static_cast<float> (min_b_[2]));
324 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
325 index_vector.emplace_back(
static_cast<unsigned int> (idx), index);
334 for (
const auto& index : (*indices_))
336 if (!input_->is_dense)
341 int ijk0 =
static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) -
static_cast<float> (min_b_[0]));
342 int ijk1 =
static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) -
static_cast<float> (min_b_[1]));
343 int ijk2 =
static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) -
static_cast<float> (min_b_[2]));
346 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
347 index_vector.emplace_back(
static_cast<unsigned int> (idx), index);
354 boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
358 unsigned int total = 0;
359 unsigned int index = 0;
363 std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
365 first_and_last_indices_vector.reserve (index_vector.size ());
366 while (index < index_vector.size ())
368 unsigned int i = index + 1;
369 while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
371 if (i - index >= min_points_per_voxel_)
374 first_and_last_indices_vector.emplace_back(index, i);
381 if (save_leaf_layout_)
386 std::uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
388 std::uint32_t reinit_size = std::min (
static_cast<unsigned int> (new_layout_size),
static_cast<unsigned int> (leaf_layout_.size()));
389 for (std::uint32_t i = 0; i < reinit_size; i++)
391 leaf_layout_[i] = -1;
393 leaf_layout_.resize (new_layout_size, -1);
395 catch (std::bad_alloc&)
397 throw PCLException(
"VoxelGrid bin size is too low; impossible to allocate memory for layout",
398 "voxel_grid.hpp",
"applyFilter");
400 catch (std::length_error&)
402 throw PCLException(
"VoxelGrid bin size is too low; impossible to allocate memory for layout",
403 "voxel_grid.hpp",
"applyFilter");
408 for (
const auto &cp : first_and_last_indices_vector)
411 unsigned int first_index = cp.first;
412 unsigned int last_index = cp.second;
415 if (save_leaf_layout_)
416 leaf_layout_[index_vector[first_index].idx] = index;
419 if (!downsample_all_data_)
421 Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
423 for (
unsigned int li = first_index; li < last_index; ++li)
424 centroid += (*input_)[index_vector[li].cloud_point_index].getVector4fMap ();
426 centroid /=
static_cast<float> (last_index - first_index);
427 output[index].getVector4fMap () = centroid;
434 for (
unsigned int li = first_index; li < last_index; ++li)
435 centroid.
add ((*input_)[index_vector[li].cloud_point_index]);
437 centroid.
get (output[index]);
445 #define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
446 #define PCL_INSTANTIATE_getMinMax3D(T) template PCL_EXPORTS void pcl::getMinMax3D<T> (const pcl::PointCloud<T>::ConstPtr &, const std::string &, float, float, Eigen::Vector4f &, Eigen::Vector4f &, bool);
Define methods for centroid estimation and covariance matrix calculus.
A generic class that computes the centroid of points fed to it.
void get(PointOutT &point) const
Retrieve the current centroid.
void add(const PointT &point)
Add a new point to the centroid computation.
A base class for all pcl exceptions which inherits from std::runtime_error.
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 resize(std::size_t count)
Resizes the container to contain count elements.
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).
void clear()
Removes all points in a cloud and sets the width and height to 0.
shared_ptr< const PointCloud< PointT > > ConstPtr
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
Define standard C methods and C++ classes that are common to all methods.
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.
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
constexpr bool isXYZFinite(const PointT &) noexcept
unsigned int cloud_point_index
cloud_point_index_idx()=default
bool operator<(const cloud_point_index_idx &p) const
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)