40 #ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
41 #define PCL_FILTERS_VOXEL_GRID_MAP_H_
43 #include <pcl/filters/boost.h>
44 #include <pcl/filters/filter.h>
59 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
77 const std::string &distance_field_name,
float min_distance,
float max_distance,
78 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
84 inline Eigen::MatrixXi
87 Eigen::MatrixXi relative_coordinates (3, 13);
91 for (
int i = -1; i < 2; i++)
93 for (
int j = -1; j < 2; j++)
95 relative_coordinates (0, idx) = i;
96 relative_coordinates (1, idx) = j;
97 relative_coordinates (2, idx) = -1;
102 for (
int i = -1; i < 2; i++)
104 relative_coordinates (0, idx) = i;
105 relative_coordinates (1, idx) = -1;
106 relative_coordinates (2, idx) = 0;
110 relative_coordinates (0, idx) = -1;
111 relative_coordinates (1, idx) = 0;
112 relative_coordinates (2, idx) = 0;
114 return (relative_coordinates);
121 inline Eigen::MatrixXi
125 Eigen::MatrixXi relative_coordinates_all( 3, 26);
126 relative_coordinates_all.block<3, 13> (0, 0) = relative_coordinates;
127 relative_coordinates_all.block<3, 13> (0, 13) = -relative_coordinates;
128 return (relative_coordinates_all);
142 template <
typename Po
intT>
void
144 const std::string &distance_field_name,
float min_distance,
float max_distance,
145 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
159 template <
typename Po
intT>
void
161 const std::vector<int> &indices,
162 const std::string &distance_field_name,
float min_distance,
float max_distance,
163 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
177 template <
typename Po
intT>
192 typedef boost::shared_ptr< VoxelGrid<PointT> >
Ptr;
193 typedef boost::shared_ptr< const VoxelGrid<PointT> >
ConstPtr;
251 inline Eigen::Vector3f
290 inline Eigen::Vector3i
296 inline Eigen::Vector3i
302 inline Eigen::Vector3i
308 inline Eigen::Vector3i
333 inline std::vector<int>
336 Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x *
inverse_leaf_size_[0])),
339 Eigen::Array4i diff2min =
min_b_ - ijk;
340 Eigen::Array4i diff2max =
max_b_ - ijk;
341 std::vector<int> neighbors (relative_coordinates.cols());
342 for (
int ni = 0; ni < relative_coordinates.cols (); ni++)
344 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
346 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
357 inline std::vector<int>
365 inline Eigen::Vector3i
369 static_cast<int> (floor (y * inverse_leaf_size_[1])),
370 static_cast<int> (floor (z * inverse_leaf_size_[2]))));
379 int idx = ((Eigen::Vector4i() << ijk, 0).finished() -
min_b_).dot (
divb_mul_);
380 if (idx < 0 || idx >= static_cast<int> (
leaf_layout_.size ()))
400 inline std::string
const
524 leaf_size_ (
Eigen::Vector4f::Zero ()),
525 inverse_leaf_size_ (
Eigen::Array4f::Zero ()),
526 downsample_all_data_ (true),
527 save_leaf_layout_ (false),
529 min_b_ (
Eigen::Vector4i::Zero ()),
530 max_b_ (
Eigen::Vector4i::Zero ()),
531 div_b_ (
Eigen::Vector4i::Zero ()),
532 divb_mul_ (
Eigen::Vector4i::Zero ()),
533 filter_field_name_ (
""),
534 filter_limit_min_ (-FLT_MAX),
535 filter_limit_max_ (FLT_MAX),
536 filter_limit_negative_ (false),
537 min_points_per_voxel_ (0)
539 filter_name_ =
"VoxelGrid";
553 leaf_size_ = leaf_size;
555 if (leaf_size_[3] == 0)
558 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
569 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
571 if (leaf_size_[3] == 0)
574 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
578 inline Eigen::Vector3f
617 inline Eigen::Vector3i
623 inline Eigen::Vector3i
629 inline Eigen::Vector3i
635 inline Eigen::Vector3i
648 return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
649 static_cast<int> (floor (y * inverse_leaf_size_[1])),
650 static_cast<int> (floor (z * inverse_leaf_size_[2])),
652 - min_b_).dot (divb_mul_)));
663 inline std::vector<int>
666 Eigen::Vector4i ijk (static_cast<int> (floor (x * inverse_leaf_size_[0])),
667 static_cast<int> (floor (y * inverse_leaf_size_[1])),
668 static_cast<int> (floor (z * inverse_leaf_size_[2])), 0);
669 Eigen::Array4i diff2min = min_b_ - ijk;
670 Eigen::Array4i diff2max = max_b_ - ijk;
671 std::vector<int> neighbors (relative_coordinates.cols());
672 for (
int ni = 0; ni < relative_coordinates.cols (); ni++)
674 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
676 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
677 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))];
692 inline std::vector<int>
693 getNeighborCentroidIndices (
float x,
float y,
float z,
const std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &relative_coordinates)
const
695 Eigen::Vector4i ijk (static_cast<int> (floorf (x * inverse_leaf_size_[0])), static_cast<int> (floorf (y * inverse_leaf_size_[1])), static_cast<int> (floorf (z * inverse_leaf_size_[2])), 0);
696 std::vector<int> neighbors;
697 neighbors.reserve (relative_coordinates.size ());
698 for (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++)
699 neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << *it, 0).finished() - min_b_).dot (divb_mul_)]);
706 inline std::vector<int>
714 inline Eigen::Vector3i
717 return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
718 static_cast<int> (floor (y * inverse_leaf_size_[1])),
719 static_cast<int> (floor (z * inverse_leaf_size_[2]))));
728 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
729 if (idx < 0 || idx >= static_cast<int> (leaf_layout_.size ()))
735 return (leaf_layout_[idx]);
745 filter_field_name_ = field_name;
749 inline std::string
const
752 return (filter_field_name_);
762 filter_limit_min_ = limit_min;
763 filter_limit_max_ = limit_max;
773 limit_min = filter_limit_min_;
774 limit_max = filter_limit_max_;
784 filter_limit_negative_ = limit_negative;
793 limit_negative = filter_limit_negative_;
802 return (filter_limit_negative_);
828 Eigen::Vector4i
min_b_, max_b_, div_b_, divb_mul_;
849 applyFilter (PCLPointCloud2 &output);
853 #ifdef PCL_NO_PRECOMPILE
854 #include <pcl/filters/impl/voxel_grid.hpp>
857 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
void getFilterLimitsNegative(bool &limit_negative) const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false)...
PointCloud::Ptr PointCloudPtr
int getCentroidIndexAt(const Eigen::Vector3i &ijk) const
Returns the index in the downsampled cloud corresponding to a given set of coordinates.
unsigned int min_points_per_voxel_
Minimum number of points per voxel for the centroid to be computed.
void applyFilter(PointCloud &output)
Downsample a Point Cloud using a voxelized grid approach.
boost::shared_ptr< PointCloud< PointT > > Ptr
Eigen::Vector3i getDivisionMultiplier() const
Get the multipliers to be applied to the grid coordinates in order to find the centroid index (after ...
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
void setFilterLimitsNegative(const bool limit_negative)
Set to true if we want to return the data outside the interval specified by setFilterLimits (min...
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
std::string const getFilterFieldName() const
Get the name of the field used for filtering.
Eigen::Vector3i getNrDivisions() const
Get the number of divisions along all 3 axes (after filtering is performed).
std::vector< int > leaf_layout_
The leaf layout information for fast access to cells relative to current position.
unsigned int min_points_per_voxel_
Minimum number of points per voxel for the centroid to be computed.
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Eigen::Vector3i getGridCoordinates(float x, float y, float z) const
Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
This file defines compatibility wrappers for low level I/O functions.
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
unsigned int getMinimumPointsNumberPerVoxel() const
Return the minimum number of points required for a voxel to be used.
int getCentroidIndexAt(const Eigen::Vector3i &ijk) const
Returns the index in the downsampled cloud corresponding to a given set of coordinates.
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
Eigen::Vector4f leaf_size_
The size of a leaf.
Eigen::Vector3i getMinBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
boost::shared_ptr< VoxelGrid< PointT > > Ptr
bool getDownsampleAllData() const
Get the state of the internal downsampling parameter (true if all fields need to be downsampled...
pcl::traits::fieldList< PointT >::type FieldList
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
boost::shared_ptr< ::pcl::PCLPointCloud2 const > PCLPointCloud2ConstPtr
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
std::string const getFilterFieldName() const
Get the name of the field used for filtering.
VoxelGrid()
Empty constructor.
void getFilterLimits(double &limit_min, double &limit_max) const
Get the field filter limits (min/max) set by the user.
std::vector< int > getNeighborCentroidIndices(float x, float y, float z, const std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > &relative_coordinates) const
Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinate...
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
bool getDownsampleAllData() const
Get the state of the internal downsampling parameter (true if all fields need to be downsampled...
Eigen::Vector3i getNrDivisions() const
Get the number of divisions along all 3 axes (after filtering is performed).
std::string filter_field_name_
The desired user filter field name.
bool getFilterLimitsNegative() const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false)...
bool filter_limit_negative_
Set to true if we want to return the data outside (filter_limit_min_;filter_limit_max_).
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
virtual ~VoxelGrid()
Destructor.
std::vector< int > getNeighborCentroidIndices(float x, float y, float z, const Eigen::MatrixXi &relative_coordinates) const
Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinate...
bool getFilterLimitsNegative() const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false)...
unsigned int getMinimumPointsNumberPerVoxel() const
Return the minimum number of points required for a voxel to be used.
Eigen::Vector3i getDivisionMultiplier() const
Get the multipliers to be applied to the grid coordinates in order to find the centroid index (after ...
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Eigen::Vector3i getMaxBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
Eigen::Vector3i getGridCoordinates(float x, float y, float z) const
Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
std::vector< int > getLeafLayout() const
Returns the layout of the leafs for fast access to cells relative to current position.
Filter represents the base filter class.
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
Eigen::Vector3i getMaxBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
void setMinimumPointsNumberPerVoxel(unsigned int min_points_per_voxel)
Set the minimum number of points required for a voxel to be used.
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Eigen::Vector3f getLeafSize() const
Get the voxel grid leaf size.
bool getSaveLeafLayout() const
Returns true if leaf layout information will to be saved for later access.
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...
Eigen::MatrixXi getHalfNeighborCellIndices()
Get the relative cell indices of the "upper half" 13 neighbors.
void setFilterLimitsNegative(const bool limit_negative)
Set to true if we want to return the data outside the interval specified by setFilterLimits (min...
std::vector< int > getLeafLayout() const
Returns the layout of the leafs for fast access to cells relative to current position.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
double filter_limit_max_
The maximum allowed filter value a point will be considered from.
Eigen::Vector4i divb_mul_
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void getFilterLimits(double &limit_min, double &limit_max) const
Get the field filter limits (min/max) set by the user.
Eigen::Vector4f leaf_size_
The size of a leaf.
boost::shared_ptr< ::pcl::PCLPointCloud2 > PCLPointCloud2Ptr
std::vector< int > leaf_layout_
The leaf layout information for fast access to cells relative to current position.
void setMinimumPointsNumberPerVoxel(unsigned int min_points_per_voxel)
Set the minimum number of points required for a voxel to be used.
std::string filter_field_name_
The desired user filter field name.
VoxelGrid()
Empty constructor.
bool filter_limit_negative_
Set to true if we want to return the data outside (filter_limit_min_;filter_limit_max_).
int getCentroidIndex(float x, float y, float z) const
Returns the index in the resulting downsampled cloud of the specified point.
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout.
boost::shared_ptr< const VoxelGrid< PointT > > ConstPtr
std::vector< int > getNeighborCentroidIndices(const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates) const
Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinate...
Eigen::MatrixXi getAllNeighborCellIndices()
Get the relative cell indices of all the 26 neighbors.
bool getSaveLeafLayout() const
Returns true if leaf layout information will to be saved for later access.
Eigen::Vector3f getLeafSize() const
Get the voxel grid leaf size.
Eigen::Vector3i getMinBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
double filter_limit_min_
The minimum allowed filter value a point will be considered from.
double filter_limit_max_
The maximum allowed filter value a point will be considered from.
PointCloud::ConstPtr PointCloudConstPtr
std::string filter_name_
The filter name.
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
double filter_limit_min_
The minimum allowed filter value a point will be considered from.
void getFilterLimitsNegative(bool &limit_negative) const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false)...
virtual ~VoxelGrid()
Destructor.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Filter< PointT >::PointCloud PointCloud