42 #include <pcl/filters/boost.h> 43 #include <pcl/filters/filter.h> 58 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
76 const std::string &distance_field_name,
float min_distance,
float max_distance,
77 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
83 inline Eigen::MatrixXi
86 Eigen::MatrixXi relative_coordinates (3, 13);
90 for (
int i = -1; i < 2; i++)
92 for (
int j = -1; j < 2; j++)
94 relative_coordinates (0, idx) = i;
95 relative_coordinates (1, idx) = j;
96 relative_coordinates (2, idx) = -1;
101 for (
int i = -1; i < 2; i++)
103 relative_coordinates (0, idx) = i;
104 relative_coordinates (1, idx) = -1;
105 relative_coordinates (2, idx) = 0;
109 relative_coordinates (0, idx) = -1;
110 relative_coordinates (1, idx) = 0;
111 relative_coordinates (2, idx) = 0;
113 return (relative_coordinates);
120 inline Eigen::MatrixXi
124 Eigen::MatrixXi relative_coordinates_all( 3, 26);
125 relative_coordinates_all.block<3, 13> (0, 0) = relative_coordinates;
126 relative_coordinates_all.block<3, 13> (0, 13) = -relative_coordinates;
127 return (relative_coordinates_all);
141 template <
typename Po
intT>
void 143 const std::string &distance_field_name,
float min_distance,
float max_distance,
144 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
158 template <
typename Po
intT>
void 160 const std::vector<int> &indices,
161 const std::string &distance_field_name,
float min_distance,
float max_distance,
162 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
176 template <
typename Po
intT>
191 using Ptr = shared_ptr<VoxelGrid<PointT> >;
192 using ConstPtr = shared_ptr<const VoxelGrid<PointT> >;
249 inline Eigen::Vector3f
288 inline Eigen::Vector3i
294 inline Eigen::Vector3i
300 inline Eigen::Vector3i
306 inline Eigen::Vector3i
331 inline std::vector<int>
334 Eigen::Vector4i ijk (static_cast<int> (std::floor (reference_point.x *
inverse_leaf_size_[0])),
337 Eigen::Array4i diff2min =
min_b_ - ijk;
338 Eigen::Array4i diff2max =
max_b_ - ijk;
339 std::vector<int> neighbors (relative_coordinates.cols());
340 for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++)
342 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
344 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
355 inline std::vector<int>
363 inline Eigen::Vector3i
377 int idx = ((Eigen::Vector4i() << ijk, 0).finished() -
min_b_).dot (
divb_mul_);
378 if (idx < 0 || idx >= static_cast<int> (
leaf_layout_.size ()))
398 inline std::string
const 488 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
522 leaf_size_ (
Eigen::Vector4f::Zero ()),
523 inverse_leaf_size_ (
Eigen::Array4f::Zero ()),
524 downsample_all_data_ (true),
525 save_leaf_layout_ (false),
526 min_b_ (
Eigen::Vector4i::Zero ()),
527 max_b_ (
Eigen::Vector4i::Zero ()),
528 div_b_ (
Eigen::Vector4i::Zero ()),
529 divb_mul_ (
Eigen::Vector4i::Zero ()),
530 filter_field_name_ (
""),
531 filter_limit_min_ (-FLT_MAX),
532 filter_limit_max_ (FLT_MAX),
533 filter_limit_negative_ (false),
534 min_points_per_voxel_ (0)
536 filter_name_ =
"VoxelGrid";
550 leaf_size_ = leaf_size;
552 if (leaf_size_[3] == 0)
555 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
566 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
568 if (leaf_size_[3] == 0)
571 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
575 inline Eigen::Vector3f
614 inline Eigen::Vector3i
620 inline Eigen::Vector3i
626 inline Eigen::Vector3i
632 inline Eigen::Vector3i
645 return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
646 static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
647 static_cast<int> (std::floor (z * inverse_leaf_size_[2])),
649 - min_b_).dot (divb_mul_)));
660 inline std::vector<int>
663 Eigen::Vector4i ijk (static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
664 static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
665 static_cast<int> (std::floor (z * inverse_leaf_size_[2])), 0);
666 Eigen::Array4i diff2min = min_b_ - ijk;
667 Eigen::Array4i diff2max = max_b_ - ijk;
668 std::vector<int> neighbors (relative_coordinates.cols());
669 for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++)
671 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
673 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
674 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))];
689 inline std::vector<int>
690 getNeighborCentroidIndices (
float x,
float y,
float z,
const std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &relative_coordinates)
const 692 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);
693 std::vector<int> neighbors;
694 neighbors.reserve (relative_coordinates.size ());
695 for (
const auto &relative_coordinate : relative_coordinates)
696 neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << relative_coordinate, 0).finished() - min_b_).dot (divb_mul_)]);
703 inline std::vector<int>
711 inline Eigen::Vector3i
714 return (Eigen::Vector3i (static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
715 static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
716 static_cast<int> (std::floor (z * inverse_leaf_size_[2]))));
725 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
726 if (idx < 0 || idx >= static_cast<int> (leaf_layout_.size ()))
732 return (leaf_layout_[idx]);
742 filter_field_name_ = field_name;
746 inline std::string
const 749 return (filter_field_name_);
759 filter_limit_min_ = limit_min;
760 filter_limit_max_ = limit_max;
770 limit_min = filter_limit_min_;
771 limit_max = filter_limit_max_;
781 filter_limit_negative_ = limit_negative;
790 limit_negative = filter_limit_negative_;
799 return (filter_limit_negative_);
825 Eigen::Vector4i
min_b_, max_b_, div_b_, divb_mul_;
850 #ifdef PCL_NO_PRECOMPILE 851 #include <pcl/filters/impl/voxel_grid.hpp> bool getDownsampleAllData() const
Get the state of the internal downsampling parameter (true if all fields need to be downsampled...
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
unsigned int min_points_per_voxel_
Minimum number of points per voxel for the centroid to be computed.
Eigen::Vector3i getMinBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
void getFilterLimitsNegative(bool &limit_negative) const
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.
shared_ptr< PointCloud< PointT > > Ptr
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).
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
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.
int getCentroidIndexAt(const Eigen::Vector3i &ijk) const
Returns the index in the downsampled cloud corresponding to a given set of coordinates.
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
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.
unsigned int getMinimumPointsNumberPerVoxel() const
Return the minimum number of points required for a voxel to be used.
Eigen::Vector4f leaf_size_
The size of a leaf.
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
bool getSaveLeafLayout() const
Returns true if leaf layout information will to be saved for later access.
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...
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
VoxelGrid()
Empty constructor.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
typename pcl::traits::fieldList< pcl::PointXYZRGBL >::type FieldList
bool getFilterLimitsNegative() const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false)...
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
bool getDownsampleAllData() const
Get the state of the internal downsampling parameter (true if all fields need to be downsampled...
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
Eigen::Vector3i getDivisionMultiplier() const
Get the multipliers to be applied to the grid coordinates in order to find the centroid index (after ...
std::string filter_field_name_
The desired user filter field name.
bool filter_limit_negative_
Set to true if we want to return the data outside (filter_limit_min_;filter_limit_max_).
PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr
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...
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::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Filter represents the base filter class.
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 setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
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() const
Returns the layout of the leafs for fast access to cells relative to current position.
shared_ptr< Filter< pcl::PointXYZRGBL > > Ptr
Eigen::Vector3f getLeafSize() const
Get the voxel grid leaf size.
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.
typename PointCloud::Ptr PointCloudPtr
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...
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
void getFilterLimitsNegative(bool &limit_negative) const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false)...
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.
double filter_limit_max_
The maximum allowed filter value a point will be considered from.
Eigen::Vector4i divb_mul_
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
Eigen::Vector3i getMinBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
Eigen::Vector4f leaf_size_
The size of a leaf.
std::vector< int > leaf_layout_
The leaf layout information for fast access to cells relative to current position.
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.
std::string filter_field_name_
The desired user filter field name.
bool getSaveLeafLayout() const
Returns true if leaf layout information will to be saved for later access.
VoxelGrid()
Empty constructor.
Eigen::Vector3i getDivisionMultiplier() const
Get the multipliers to be applied to the grid coordinates in order to find the centroid index (after ...
bool filter_limit_negative_
Set to true if we want to return the data outside (filter_limit_min_;filter_limit_max_).
void getFilterLimits(double &limit_min, double &limit_max) const
Get the field filter limits (min/max) set by the user.
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout.
PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr
std::string const getFilterFieldName() const
Get the name of the field used for filtering.
shared_ptr< const PointCloud< PointT > > ConstPtr
Eigen::MatrixXi getAllNeighborCellIndices()
Get the relative cell indices of all the 26 neighbors.
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
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.
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.
int getCentroidIndex(float x, float y, float z) const
Returns the index in the resulting downsampled cloud of the specified point.
unsigned int getMinimumPointsNumberPerVoxel() const
Return the minimum number of points required for a voxel to be used.
std::string filter_name_
The filter name.
Eigen::Vector3f getLeafSize() const
Get the voxel grid leaf size.
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
shared_ptr< const Filter< pcl::PointXYZRGBL > > ConstPtr
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.
Eigen::Vector3i getNrDivisions() const
Get the number of divisions along all 3 axes (after filtering is performed).
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).
PCLPointCloud2::Ptr PCLPointCloud2Ptr
typename PointCloud::ConstPtr PointCloudConstPtr
void getFilterLimits(double &limit_min, double &limit_max) const
Get the field filter limits (min/max) set by the user.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.