42 #include <pcl/filters/filter.h>
57 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
75 const std::string &distance_field_name,
float min_distance,
float max_distance,
76 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
82 inline Eigen::MatrixXi
85 Eigen::MatrixXi relative_coordinates (3, 13);
89 for (
int i = -1; i < 2; i++)
91 for (
int j = -1; j < 2; j++)
93 relative_coordinates (0, idx) = i;
94 relative_coordinates (1, idx) = j;
95 relative_coordinates (2, idx) = -1;
100 for (
int i = -1; i < 2; i++)
102 relative_coordinates (0, idx) = i;
103 relative_coordinates (1, idx) = -1;
104 relative_coordinates (2, idx) = 0;
108 relative_coordinates (0, idx) = -1;
109 relative_coordinates (1, idx) = 0;
110 relative_coordinates (2, idx) = 0;
112 return (relative_coordinates);
119 inline Eigen::MatrixXi
123 Eigen::MatrixXi relative_coordinates_all( 3, 26);
124 relative_coordinates_all.block<3, 13> (0, 0) = relative_coordinates;
125 relative_coordinates_all.block<3, 13> (0, 13) = -relative_coordinates;
126 return (relative_coordinates_all);
140 template <
typename Po
intT>
void
142 const std::string &distance_field_name,
float min_distance,
float max_distance,
143 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
157 template <
typename Po
intT>
void
160 const std::string &distance_field_name,
float min_distance,
float max_distance,
161 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
175 template <
typename Po
intT>
190 using Ptr = shared_ptr<VoxelGrid<PointT> >;
191 using ConstPtr = shared_ptr<const VoxelGrid<PointT> >;
241 inline Eigen::Vector3f
280 inline Eigen::Vector3i
286 inline Eigen::Vector3i
292 inline Eigen::Vector3i
298 inline Eigen::Vector3i
323 inline std::vector<int>
326 Eigen::Vector4i ijk (
static_cast<int> (std::floor (reference_point.x *
inverse_leaf_size_[0])),
329 Eigen::Array4i diff2min =
min_b_ - ijk;
330 Eigen::Array4i diff2max =
max_b_ - ijk;
331 std::vector<int> neighbors (relative_coordinates.cols());
332 for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++)
334 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
336 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
347 inline std::vector<int>
355 inline Eigen::Vector3i
369 int idx = ((Eigen::Vector4i() << ijk, 0).finished() -
min_b_).dot (
divb_mul_);
370 if (idx < 0 || idx >=
static_cast<int> (
leaf_layout_.size ()))
390 inline std::string
const
432 PCL_DEPRECATED(1, 16,
"use bool getFilterLimitsNegative() instead")
482 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
516 leaf_size_ (
Eigen::Vector4f::Zero ()),
517 inverse_leaf_size_ (
Eigen::Array4f::Zero ()),
519 min_b_ (
Eigen::Vector4i::Zero ()),
520 max_b_ (
Eigen::Vector4i::Zero ()),
521 div_b_ (
Eigen::Vector4i::Zero ()),
522 divb_mul_ (
Eigen::Vector4i::Zero ())
524 filter_name_ =
"VoxelGrid";
536 leaf_size_ = leaf_size;
538 if (leaf_size_[3] == 0)
541 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
552 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
554 if (leaf_size_[3] == 0)
557 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
561 inline Eigen::Vector3f
600 inline Eigen::Vector3i
606 inline Eigen::Vector3i
612 inline Eigen::Vector3i
618 inline Eigen::Vector3i
631 return (leaf_layout_.at ((Eigen::Vector4i (
static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
632 static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
633 static_cast<int> (std::floor (z * inverse_leaf_size_[2])),
635 - min_b_).dot (divb_mul_)));
646 inline std::vector<int>
649 Eigen::Vector4i ijk (
static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
650 static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
651 static_cast<int> (std::floor (z * inverse_leaf_size_[2])), 0);
652 Eigen::Array4i diff2min = min_b_ - ijk;
653 Eigen::Array4i diff2max = max_b_ - ijk;
654 std::vector<int> neighbors (relative_coordinates.cols());
655 for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++)
657 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
659 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
660 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))];
675 inline std::vector<int>
676 getNeighborCentroidIndices (
float x,
float y,
float z,
const std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &relative_coordinates)
const
678 Eigen::Vector4i ijk (
static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
static_cast<int> (std::floor (z * inverse_leaf_size_[2])), 0);
679 std::vector<int> neighbors;
680 neighbors.reserve (relative_coordinates.size ());
681 for (
const auto &relative_coordinate : relative_coordinates)
682 neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << relative_coordinate, 0).finished() - min_b_).dot (divb_mul_)]);
689 inline std::vector<int>
697 inline Eigen::Vector3i
700 return {
static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
701 static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
702 static_cast<int> (std::floor (z * inverse_leaf_size_[2]))};
711 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
712 if (idx < 0 || idx >=
static_cast<int> (leaf_layout_.size ()))
718 return (leaf_layout_[idx]);
728 filter_field_name_ = field_name;
732 inline std::string
const
735 return (filter_field_name_);
745 filter_limit_min_ = limit_min;
746 filter_limit_max_ = limit_max;
757 limit_min = filter_limit_min_;
758 limit_max = filter_limit_max_;
768 filter_limit_negative_ = limit_negative;
774 PCL_DEPRECATED(1, 16,
"use bool getFilterLimitsNegative() instead")
776 getFilterLimitsNegative (
bool &limit_negative)
const
778 limit_negative = filter_limit_negative_;
787 return (filter_limit_negative_);
798 bool downsample_all_data_{
true};
803 bool save_leaf_layout_{
false};
813 Eigen::Vector4i min_b_, max_b_,
div_b_, divb_mul_;
819 double filter_limit_min_{std::numeric_limits<float>::lowest()};
822 double filter_limit_max_{std::numeric_limits<float>::max()};
825 bool filter_limit_negative_{
false};
828 unsigned int min_points_per_voxel_{0};
838 #ifdef PCL_NO_PRECOMPILE
839 #include <pcl/filters/impl/voxel_grid.hpp>
Filter represents the base filter class.
shared_ptr< Filter< PointT > > Ptr
shared_ptr< const Filter< PointT > > ConstPtr
std::string filter_name_
The filter name.
PCLPointCloud2::Ptr PCLPointCloud2Ptr
PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr
typename PointCloud::Ptr PointCloudPtr
typename PointCloud::ConstPtr PointCloudConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
int getCentroidIndexAt(const Eigen::Vector3i &ijk) const
Returns the index in the downsampled cloud corresponding to a given set of coordinates.
Eigen::Vector3i getDivisionMultiplier() const
Get the multipliers to be applied to the grid coordinates in order to find the centroid index (after ...
bool getFilterLimitsNegative() const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
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...
std::string filter_field_name_
The desired user filter field name.
std::vector< int > leaf_layout_
The leaf layout information for fast access to cells relative to current position.
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
Eigen::Vector3f getLeafSize() const
Get the voxel grid leaf size.
Eigen::Vector3i getNrDivisions() const
Get the number of divisions along all 3 axes (after filtering is performed).
Eigen::Vector3i getMinBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
void applyFilter(PCLPointCloud2 &output) override
Downsample a Point Cloud using a voxelized grid approach.
unsigned int getMinimumPointsNumberPerVoxel() const
Return the minimum number of points required for a voxel to be used.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
void getFilterLimits(double &limit_min, double &limit_max) const
Get the field filter limits (min/max) set by the user.
VoxelGrid()
Empty constructor.
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...
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).
bool getDownsampleAllData() const
Get the state of the internal downsampling parameter (true if all fields need to be downsampled,...
bool getSaveLeafLayout() const
Returns true if leaf layout information will to be saved for later access.
std::vector< int > getLeafLayout() const
Returns the layout of the leafs for fast access to cells relative to current position.
std::string const getFilterFieldName() const
Get the name of the field used for filtering.
Eigen::Vector3i getMaxBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
void setFilterLimitsNegative(const bool limit_negative)
Set to true if we want to return the data outside the interval specified by setFilterLimits (min,...
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
~VoxelGrid() override=default
Destructor.
void setMinimumPointsNumberPerVoxel(unsigned int min_points_per_voxel)
Set the minimum number of points required for a voxel to be used.
int getCentroidIndex(float x, float y, float z) const
Returns the index in the resulting downsampled cloud of the specified point.
Eigen::Vector4f leaf_size_
The size of a leaf.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
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).
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Eigen::Vector3i getDivisionMultiplier() const
Get the multipliers to be applied to the grid coordinates in order to find the centroid index (after ...
bool getFilterLimitsNegative() const
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,...
Eigen::Vector3i getMaxBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
unsigned int min_points_per_voxel_
Minimum number of points per voxel for the centroid to be computed.
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
unsigned int getMinimumPointsNumberPerVoxel() const
Return the minimum number of points required for a voxel to be used.
bool getDownsampleAllData() const
Get the state of the internal downsampling parameter (true if all fields need to be downsampled,...
std::vector< int > leaf_layout_
The leaf layout information for fast access to cells relative to current position.
void getFilterLimits(double &limit_min, double &limit_max) const
Get the field filter limits (min/max) set by the user.
Eigen::Vector4i divb_mul_
typename pcl::traits::fieldList< PointT >::type FieldList
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
Eigen::Vector3i getNrDivisions() const
Get the number of divisions along all 3 axes (after filtering is performed).
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
bool filter_limit_negative_
Set to true if we want to return the data outside (filter_limit_min_;filter_limit_max_).
std::vector< int > getLeafLayout() const
Returns the layout of the leafs for fast access to cells relative to current position.
int getCentroidIndexAt(const Eigen::Vector3i &ijk) const
Returns the index in the downsampled cloud corresponding to a given set of coordinates.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
double filter_limit_max_
The maximum allowed filter value a point will be considered from.
void setMinimumPointsNumberPerVoxel(unsigned int min_points_per_voxel)
Set the minimum number of points required for a voxel to be used.
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...
PCL_MAKE_ALIGNED_OPERATOR_NEW VoxelGrid()
Empty constructor.
std::string filter_field_name_
The desired user filter field name.
Eigen::Vector4f leaf_size_
The size of a leaf.
double filter_limit_min_
The minimum allowed filter value a point will be considered from.
Eigen::Vector3f getLeafSize() const
Get the voxel grid leaf size.
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
std::string const getFilterFieldName() const
Get the name of the field used for filtering.
Eigen::Vector3i getMinBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
~VoxelGrid() override=default
Destructor.
bool getSaveLeafLayout() const
Returns true if leaf layout information will to be saved for later access.
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
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.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Eigen::MatrixXi getAllNeighborCellIndices()
Get the relative cell indices of all the 26 neighbors.
Eigen::MatrixXi getHalfNeighborCellIndices()
Get the relative cell indices of the "upper half" 13 neighbors.
IndicesAllocator<> Indices
Type used for indices in PCL.
PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major....
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.