38 #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_H_ 39 #define PCL_FILTERS_IMPL_VOXEL_GRID_H_ 43 #include <pcl/common/io.h> 44 #include <pcl/filters/voxel_grid.h> 47 template <
typename Po
intT>
void 49 const std::string &distance_field_name,
float min_distance,
float max_distance,
50 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative)
52 Eigen::Array4f min_p, max_p;
53 min_p.setConstant (FLT_MAX);
54 max_p.setConstant (-FLT_MAX);
57 std::vector<pcl::PCLPointField> fields;
64 for (
size_t i = 0; i < cloud->
points.size (); ++i)
67 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->
points[i]);
68 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
73 if ((distance_value < max_distance) && (distance_value > min_distance))
79 if ((distance_value > max_distance) || (distance_value < min_distance))
84 min_p = min_p.min (pt);
85 max_p = max_p.max (pt);
90 for (
size_t i = 0; i < cloud->
points.size (); ++i)
93 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->
points[i]);
94 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
99 if ((distance_value < max_distance) && (distance_value > min_distance))
105 if ((distance_value > max_distance) || (distance_value < min_distance))
110 if (!pcl_isfinite (cloud->
points[i].x) ||
111 !pcl_isfinite (cloud->
points[i].y) ||
112 !pcl_isfinite (cloud->
points[i].z))
116 min_p = min_p.min (pt);
117 max_p = max_p.max (pt);
125 template <
typename Po
intT>
void 127 const std::vector<int> &indices,
128 const std::string &distance_field_name,
float min_distance,
float max_distance,
129 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative)
131 Eigen::Array4f min_p, max_p;
132 min_p.setConstant (FLT_MAX);
133 max_p.setConstant (-FLT_MAX);
136 std::vector<pcl::PCLPointField> fields;
139 float distance_value;
143 for (std::vector<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
146 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->
points[*it]);
147 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
152 if ((distance_value < max_distance) && (distance_value > min_distance))
158 if ((distance_value > max_distance) || (distance_value < min_distance))
163 min_p = min_p.min (pt);
164 max_p = max_p.max (pt);
169 for (std::vector<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
172 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->
points[*it]);
173 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
178 if ((distance_value < max_distance) && (distance_value > min_distance))
184 if ((distance_value > max_distance) || (distance_value < min_distance))
189 if (!pcl_isfinite (cloud->
points[*it].x) ||
190 !pcl_isfinite (cloud->
points[*it].y) ||
191 !pcl_isfinite (cloud->
points[*it].z))
195 min_p = min_p.min (pt);
196 max_p = max_p.max (pt);
213 template <
typename Po
intT>
void 219 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
229 Eigen::Vector4f min_p, max_p;
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_);
234 getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
237 int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
238 int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
239 int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
241 if ((dx*dy*dz) > static_cast<int64_t>(std::numeric_limits<int32_t>::max()))
243 PCL_WARN(
"[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
249 min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
250 max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
251 min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
252 max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
253 min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
254 max_b_[2] = static_cast<int> (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);
264 std::vector<cloud_point_index_idx> index_vector;
265 index_vector.reserve (indices_->size ());
268 if (!filter_field_name_.empty ())
271 std::vector<pcl::PCLPointField> fields;
273 if (distance_idx == -1)
274 PCL_WARN (
"[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
279 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
281 if (!input_->is_dense)
283 if (!pcl_isfinite (input_->points[*it].x) ||
284 !pcl_isfinite (input_->points[*it].y) ||
285 !pcl_isfinite (input_->points[*it].z))
289 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&input_->points[*it]);
290 float distance_value = 0;
291 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
293 if (filter_limit_negative_)
296 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
302 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
306 int ijk0 = static_cast<int> (floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
307 int ijk1 = static_cast<int> (floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
308 int ijk2 = static_cast<int> (floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
311 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
321 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
323 if (!input_->is_dense)
325 if (!pcl_isfinite (input_->points[*it].x) ||
326 !pcl_isfinite (input_->points[*it].y) ||
327 !pcl_isfinite (input_->points[*it].z))
330 int ijk0 = static_cast<int> (floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
331 int ijk1 = static_cast<int> (floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
332 int ijk2 = static_cast<int> (floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
335 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
342 std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ());
346 unsigned int total = 0;
347 unsigned int index = 0;
351 std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
353 first_and_last_indices_vector.reserve (index_vector.size ());
354 while (index < index_vector.size ())
356 unsigned int i = index + 1;
357 while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
359 if (i - index >= min_points_per_voxel_)
362 first_and_last_indices_vector.push_back (std::pair<unsigned int, unsigned int> (index, i));
368 output.
points.resize (total);
369 if (save_leaf_layout_)
374 uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
376 uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
377 for (uint32_t i = 0; i < reinit_size; i++)
379 leaf_layout_[i] = -1;
381 leaf_layout_.resize (new_layout_size, -1);
383 catch (std::bad_alloc&)
385 throw PCLException(
"VoxelGrid bin size is too low; impossible to allocate memory for layout",
386 "voxel_grid.hpp",
"applyFilter");
388 catch (std::length_error&)
390 throw PCLException(
"VoxelGrid bin size is too low; impossible to allocate memory for layout",
391 "voxel_grid.hpp",
"applyFilter");
396 for (
unsigned int cp = 0; cp < first_and_last_indices_vector.size (); ++cp)
399 unsigned int first_index = first_and_last_indices_vector[cp].first;
400 unsigned int last_index = first_and_last_indices_vector[cp].second;
403 if (save_leaf_layout_)
404 leaf_layout_[index_vector[first_index].idx] = index;
407 if (!downsample_all_data_)
409 Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
411 for (
unsigned int li = first_index; li < last_index; ++li)
412 centroid += input_->points[index_vector[li].cloud_point_index].getVector4fMap ();
414 centroid /= static_cast<float> (last_index - first_index);
415 output.
points[index].getVector4fMap () = centroid;
422 for (
unsigned int li = first_index; li < last_index; ++li)
423 centroid.
add (input_->points[index_vector[li].cloud_point_index]);
430 output.
width = static_cast<uint32_t> (output.
points.size ());
433 #define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>; 434 #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); 436 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_ const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
void applyFilter(PointCloud &output)
Downsample a Point Cloud using a voxelized grid approach.
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
A base class for all pcl exceptions which inherits from std::runtime_error.
uint32_t height
The point cloud height (if organized as an image-structure).
bool operator<(const cloud_point_index_idx &p) const
Define standard C methods and C++ classes that are common to all methods.
void add(const PointT &point)
Add a new point to the centroid computation.
uint32_t width
The point cloud width (if organized as an image-structure).
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.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
unsigned int cloud_point_index
void get(PointOutT &point) const
Retrieve the current centroid.
A generic class that computes the centroid of points fed to it.
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
Define methods for centroid estimation and covariance matrix calculus.