38 #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_H_
39 #define PCL_FILTERS_IMPL_VOXEL_GRID_H_
41 #include <pcl/common/centroid.h>
42 #include <pcl/common/common.h>
43 #include <pcl/common/io.h>
44 #include <pcl/filters/voxel_grid.h>
45 #include <boost/sort/spreadsort/integer_sort.hpp>
48 template <
typename Po
intT>
void
50 const std::string &distance_field_name,
float min_distance,
float max_distance,
51 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative)
53 Eigen::Array4f min_p, max_p;
54 min_p.setConstant (FLT_MAX);
55 max_p.setConstant (-FLT_MAX);
58 std::vector<pcl::PCLPointField> fields;
59 int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
65 for (std::size_t i = 0; i < cloud->
points.size (); ++i)
68 const std::uint8_t* pt_data =
reinterpret_cast<const std::uint8_t*
> (&cloud->
points[i]);
69 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
74 if ((distance_value < max_distance) && (distance_value > min_distance))
80 if ((distance_value > max_distance) || (distance_value < min_distance))
85 min_p = min_p.min (pt);
86 max_p = max_p.max (pt);
91 for (std::size_t i = 0; i < cloud->
points.size (); ++i)
94 const std::uint8_t* pt_data =
reinterpret_cast<const std::uint8_t*
> (&cloud->
points[i]);
95 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
100 if ((distance_value < max_distance) && (distance_value > min_distance))
106 if ((distance_value > max_distance) || (distance_value < min_distance))
111 if (!std::isfinite (cloud->
points[i].x) ||
112 !std::isfinite (cloud->
points[i].y) ||
113 !std::isfinite (cloud->
points[i].z))
117 min_p = min_p.min (pt);
118 max_p = max_p.max (pt);
126 template <
typename Po
intT>
void
128 const std::vector<int> &indices,
129 const std::string &distance_field_name,
float min_distance,
float max_distance,
130 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative)
132 Eigen::Array4f min_p, max_p;
133 min_p.setConstant (FLT_MAX);
134 max_p.setConstant (-FLT_MAX);
137 std::vector<pcl::PCLPointField> fields;
138 int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
140 float distance_value;
144 for (
const int &index : indices)
147 const std::uint8_t* pt_data =
reinterpret_cast<const std::uint8_t*
> (&cloud->
points[index]);
148 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
153 if ((distance_value < max_distance) && (distance_value > min_distance))
159 if ((distance_value > max_distance) || (distance_value < min_distance))
164 min_p = min_p.min (pt);
165 max_p = max_p.max (pt);
170 for (
const int &index : indices)
173 const std::uint8_t* pt_data =
reinterpret_cast<const std::uint8_t*
> (&cloud->
points[index]);
174 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
179 if ((distance_value < max_distance) && (distance_value > min_distance))
185 if ((distance_value > max_distance) || (distance_value < min_distance))
190 if (!std::isfinite (cloud->
points[index].x) ||
191 !std::isfinite (cloud->
points[index].y) ||
192 !std::isfinite (cloud->
points[index].z))
196 min_p = min_p.min (pt);
197 max_p = max_p.max (pt);
210 cloud_point_index_idx (
unsigned int idx_,
unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
215 template <
typename Po
intT>
void
221 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
231 Eigen::Vector4f min_p, max_p;
233 if (!filter_field_name_.empty ())
234 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_);
236 getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
239 std::int64_t dx =
static_cast<std::int64_t
>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
240 std::int64_t dy =
static_cast<std::int64_t
>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
241 std::int64_t dz =
static_cast<std::int64_t
>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
243 if ((dx*dy*dz) >
static_cast<std::int64_t
>(std::numeric_limits<std::int32_t>::max()))
245 PCL_WARN(
"[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
251 min_b_[0] =
static_cast<int> (std::floor (min_p[0] * inverse_leaf_size_[0]));
252 max_b_[0] =
static_cast<int> (std::floor (max_p[0] * inverse_leaf_size_[0]));
253 min_b_[1] =
static_cast<int> (std::floor (min_p[1] * inverse_leaf_size_[1]));
254 max_b_[1] =
static_cast<int> (std::floor (max_p[1] * inverse_leaf_size_[1]));
255 min_b_[2] =
static_cast<int> (std::floor (min_p[2] * inverse_leaf_size_[2]));
256 max_b_[2] =
static_cast<int> (std::floor (max_p[2] * inverse_leaf_size_[2]));
259 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
263 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
266 std::vector<cloud_point_index_idx> index_vector;
267 index_vector.reserve (indices_->size ());
270 if (!filter_field_name_.empty ())
273 std::vector<pcl::PCLPointField> fields;
274 int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
275 if (distance_idx == -1)
276 PCL_WARN (
"[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
281 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
283 if (!input_->is_dense)
285 if (!std::isfinite (input_->points[*it].x) ||
286 !std::isfinite (input_->points[*it].y) ||
287 !std::isfinite (input_->points[*it].z))
291 const std::uint8_t* pt_data =
reinterpret_cast<const std::uint8_t*
> (&input_->points[*it]);
292 float distance_value = 0;
293 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
295 if (filter_limit_negative_)
298 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
304 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
308 int ijk0 =
static_cast<int> (std::floor (input_->points[*it].x * inverse_leaf_size_[0]) -
static_cast<float> (min_b_[0]));
309 int ijk1 =
static_cast<int> (std::floor (input_->points[*it].y * inverse_leaf_size_[1]) -
static_cast<float> (min_b_[1]));
310 int ijk2 =
static_cast<int> (std::floor (input_->points[*it].z * inverse_leaf_size_[2]) -
static_cast<float> (min_b_[2]));
313 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
314 index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
323 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
325 if (!input_->is_dense)
327 if (!std::isfinite (input_->points[*it].x) ||
328 !std::isfinite (input_->points[*it].y) ||
329 !std::isfinite (input_->points[*it].z))
332 int ijk0 =
static_cast<int> (std::floor (input_->points[*it].x * inverse_leaf_size_[0]) -
static_cast<float> (min_b_[0]));
333 int ijk1 =
static_cast<int> (std::floor (input_->points[*it].y * inverse_leaf_size_[1]) -
static_cast<float> (min_b_[1]));
334 int ijk2 =
static_cast<int> (std::floor (input_->points[*it].z * inverse_leaf_size_[2]) -
static_cast<float> (min_b_[2]));
337 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
338 index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
345 boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
349 unsigned int total = 0;
350 unsigned int index = 0;
354 std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
356 first_and_last_indices_vector.reserve (index_vector.size ());
357 while (index < index_vector.size ())
359 unsigned int i = index + 1;
360 while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
362 if (i - index >= min_points_per_voxel_)
365 first_and_last_indices_vector.emplace_back(index, i);
371 output.
points.resize (total);
372 if (save_leaf_layout_)
377 std::uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
379 std::uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
380 for (std::uint32_t i = 0; i < reinit_size; i++)
382 leaf_layout_[i] = -1;
384 leaf_layout_.resize (new_layout_size, -1);
386 catch (std::bad_alloc&)
388 throw PCLException(
"VoxelGrid bin size is too low; impossible to allocate memory for layout",
389 "voxel_grid.hpp",
"applyFilter");
391 catch (std::length_error&)
393 throw PCLException(
"VoxelGrid bin size is too low; impossible to allocate memory for layout",
394 "voxel_grid.hpp",
"applyFilter");
399 for (
const auto &cp : first_and_last_indices_vector)
402 unsigned int first_index = cp.first;
403 unsigned int last_index = cp.second;
406 if (save_leaf_layout_)
407 leaf_layout_[index_vector[first_index].idx] = index;
410 if (!downsample_all_data_)
412 Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
414 for (
unsigned int li = first_index; li < last_index; ++li)
415 centroid += input_->points[index_vector[li].cloud_point_index].getVector4fMap ();
417 centroid /=
static_cast<float> (last_index - first_index);
418 output.
points[index].getVector4fMap () = centroid;
425 for (
unsigned int li = first_index; li < last_index; ++li)
426 centroid.
add (input_->points[index_vector[li].cloud_point_index]);
433 output.
width =
static_cast<std::uint32_t
> (output.
points.size ());
436 #define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
437 #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);
439 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_
A base class for all pcl exceptions which inherits from std::runtime_error.
bool operator<(const cloud_point_index_idx &p) const
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
cloud_point_index_idx()=default
void add(const PointT &point)
Add a new point to the centroid computation.
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...
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
shared_ptr< const PointCloud< PointT > > ConstPtr
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
std::uint32_t height
The point cloud height (if organized as an image-structure).
unsigned int cloud_point_index
void get(PointOutT &point) const
Retrieve the current centroid.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
A generic class that computes the centroid of points fed to it.
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)