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>
189 typedef boost::shared_ptr< VoxelGrid<PointT> >
Ptr;
190 typedef boost::shared_ptr< const VoxelGrid<PointT> >
ConstPtr;
250 inline Eigen::Vector3f
289 inline Eigen::Vector3i
295 inline Eigen::Vector3i
301 inline Eigen::Vector3i
307 inline Eigen::Vector3i
332 inline std::vector<int>
335 Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x *
inverse_leaf_size_[0])),
338 Eigen::Array4i diff2min =
min_b_ - ijk;
339 Eigen::Array4i diff2max =
max_b_ - ijk;
340 std::vector<int> neighbors (relative_coordinates.cols());
341 for (
int ni = 0; ni < relative_coordinates.cols (); ni++)
343 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
345 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
356 inline std::vector<int>
364 inline Eigen::Vector3i
368 static_cast<int> (floor (y * inverse_leaf_size_[1])),
369 static_cast<int> (floor (z * inverse_leaf_size_[2]))));
378 int idx = ((Eigen::Vector4i() << ijk, 0).finished() -
min_b_).dot (
divb_mul_);
379 if (idx < 0 || idx >= static_cast<int> (
leaf_layout_.size ()))
399 inline std::string
const
523 leaf_size_ (
Eigen::Vector4f::Zero ()),
524 inverse_leaf_size_ (
Eigen::Array4f::Zero ()),
525 downsample_all_data_ (true),
526 save_leaf_layout_ (false),
528 min_b_ (
Eigen::Vector4i::Zero ()),
529 max_b_ (
Eigen::Vector4i::Zero ()),
530 div_b_ (
Eigen::Vector4i::Zero ()),
531 divb_mul_ (
Eigen::Vector4i::Zero ()),
532 filter_field_name_ (
""),
533 filter_limit_min_ (-FLT_MAX),
534 filter_limit_max_ (FLT_MAX),
535 filter_limit_negative_ (false),
536 min_points_per_voxel_ (0)
538 filter_name_ =
"VoxelGrid";
552 leaf_size_ = leaf_size;
554 if (leaf_size_[3] == 0)
557 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
568 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
570 if (leaf_size_[3] == 0)
573 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
577 inline Eigen::Vector3f
616 inline Eigen::Vector3i
622 inline Eigen::Vector3i
628 inline Eigen::Vector3i
634 inline Eigen::Vector3i
647 return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
648 static_cast<int> (floor (y * inverse_leaf_size_[1])),
649 static_cast<int> (floor (z * inverse_leaf_size_[2])),
651 - min_b_).dot (divb_mul_)));
662 inline std::vector<int>
665 Eigen::Vector4i ijk (static_cast<int> (floor (x * inverse_leaf_size_[0])),
666 static_cast<int> (floor (y * inverse_leaf_size_[1])),
667 static_cast<int> (floor (z * inverse_leaf_size_[2])), 0);
668 Eigen::Array4i diff2min = min_b_ - ijk;
669 Eigen::Array4i diff2max = max_b_ - ijk;
670 std::vector<int> neighbors (relative_coordinates.cols());
671 for (
int ni = 0; ni < relative_coordinates.cols (); ni++)
673 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
675 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
676 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))];
691 inline std::vector<int>
694 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);
695 std::vector<int> neighbors;
696 neighbors.reserve (relative_coordinates.size ());
697 for (std::vector<Eigen::Vector3i>::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++)
698 neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << *it, 0).finished() - min_b_).dot (divb_mul_)]);
705 inline std::vector<int>
713 inline Eigen::Vector3i
716 return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
717 static_cast<int> (floor (y * inverse_leaf_size_[1])),
718 static_cast<int> (floor (z * inverse_leaf_size_[2]))));
727 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
728 if (idx < 0 || idx >= static_cast<int> (leaf_layout_.size ()))
734 return (leaf_layout_[idx]);
744 filter_field_name_ = field_name;
748 inline std::string
const
751 return (filter_field_name_);
761 filter_limit_min_ = limit_min;
762 filter_limit_max_ = limit_max;
772 limit_min = filter_limit_min_;
773 limit_max = filter_limit_max_;
783 filter_limit_negative_ = limit_negative;
792 limit_negative = filter_limit_negative_;
801 return (filter_limit_negative_);
827 Eigen::Vector4i
min_b_, max_b_, div_b_, divb_mul_;
848 applyFilter (PCLPointCloud2 &output);
852 #ifdef PCL_NO_PRECOMPILE
853 #include <pcl/filters/impl/voxel_grid.hpp>
856 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
PointCloud::Ptr PointCloudPtr
unsigned int min_points_per_voxel_
Minimum number of points per voxel for the centroid to be computed.
Eigen::Vector3i getGridCoordinates(float x, float y, float z)
Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
void applyFilter(PointCloud &output)
Downsample a Point Cloud using a voxelized grid approach.
Eigen::Vector3f getLeafSize()
Get the voxel grid leaf size.
boost::shared_ptr< PointCloud< PointT > > Ptr
bool getDownsampleAllData()
Get the state of the internal downsampling parameter (true if all fields need to be downsampled...
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 getFilterLimitsNegative(bool &limit_negative)
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false)...
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::vector< int > getNeighborCentroidIndices(float x, float y, float z, const Eigen::MatrixXi &relative_coordinates)
Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinate...
Eigen::Vector3i getMaxBoxCoordinates()
Get the minimum coordinates of the bounding box (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 getNrDivisions()
Get the number of divisions along all 3 axes (after filtering is performed).
bool getDownsampleAllData()
Get the state of the internal downsampling parameter (true if all fields need to be downsampled...
bool getFilterLimitsNegative()
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false)...
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
Eigen::Vector4f leaf_size_
The size of a leaf.
boost::shared_ptr< VoxelGrid< PointT > > Ptr
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.
VoxelGrid()
Empty constructor.
void getFilterLimits(double &limit_min, double &limit_max)
Get the field filter limits (min/max) set by the user.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
std::vector< int > getNeighborCentroidIndices(const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates)
Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinate...
Eigen::Vector3i getNrDivisions()
Get the number of divisions along all 3 axes (after filtering is performed).
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
std::vector< int > getLeafLayout()
Returns the layout of the leafs for fast access to cells relative to current position.
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
bool getSaveLeafLayout()
Returns true if leaf layout information will to be saved for later access.
Eigen::Vector3i getDivisionMultiplier()
Get the multipliers to be applied to the grid coordinates in order to find the centroid index (after ...
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
std::string filter_field_name_
The desired user filter field name.
std::string const getFilterFieldName()
Get the name of the field used for filtering.
bool filter_limit_negative_
Set to true if we want to return the data outside (filter_limit_min_;filter_limit_max_).
Eigen::Vector3i getDivisionMultiplier()
Get the multipliers to be applied to the grid coordinates in order to find the centroid index (after ...
unsigned int getMinimumPointsNumberPerVoxel()
Return the minimum number of points required for a voxel to be used.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
virtual ~VoxelGrid()
Destructor.
std::string const getFilterFieldName()
Get the name of the field used for filtering.
int getCentroidIndex(float x, float y, float z)
Returns the index in the resulting downsampled cloud of the specified point.
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Eigen::Vector3i getMinBoxCoordinates()
Get the minimum coordinates of the bounding box (after filtering is performed).
Eigen::Vector3i getMinBoxCoordinates()
Get the minimum coordinates of the bounding box (after filtering is performed).
Filter represents the base filter class.
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
int getCentroidIndexAt(const Eigen::Vector3i &ijk)
Returns the index in the downsampled cloud corresponding to a given set of coordinates.
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.
std::vector< int > getLeafLayout()
Returns the layout of the leafs for fast access to cells relative to current position.
Eigen::Vector3i getGridCoordinates(float x, float y, float z)
Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
int getCentroidIndex(const PointT &p)
Returns the index in the resulting downsampled cloud of the specified point.
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...
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
Eigen::Vector3i getMaxBoxCoordinates()
Get the minimum coordinates of the bounding box (after filtering is performed).
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.
bool getFilterLimitsNegative()
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false)...
Eigen::Vector4i divb_mul_
std::vector< int > getNeighborCentroidIndices(float x, float y, float z, const std::vector< Eigen::Vector3i > &relative_coordinates)
Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinate...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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_).
void getFilterLimitsNegative(bool &limit_negative)
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false)...
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
Eigen::MatrixXi getAllNeighborCellIndices()
Get the relative cell indices of all the 26 neighbors.
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.
int getCentroidIndexAt(const Eigen::Vector3i &ijk)
Returns the index in the downsampled cloud corresponding to a given set of coordinates.
unsigned int getMinimumPointsNumberPerVoxel()
Return the minimum number of points required for a voxel to be used.
virtual ~VoxelGrid()
Destructor.
bool getSaveLeafLayout()
Returns true if leaf layout information will to be saved for later access.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Filter< PointT >::PointCloud PointCloud
void getFilterLimits(double &limit_min, double &limit_max)
Get the field filter limits (min/max) set by the user.
Eigen::Vector3f getLeafSize()
Get the voxel grid leaf size.