40#include <pcl/filters/voxel_grid.h>
43#include <pcl/kdtree/kdtree_flann.h>
54 template<
typename Po
intT>
78 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
85 using Ptr = shared_ptr<VoxelGrid<PointT> >;
86 using ConstPtr = shared_ptr<const VoxelGrid<PointT> >;
216 if(min_points_per_voxel > 2)
222 PCL_WARN (
"[%s::setMinPointPerVoxel] Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3\n", this->
getClassName ().c_str ());
269 PCL_WARN (
"[%s::filter] No voxels with a sufficient number of points. Grid will not be searchable. You can try reducing the min number of points required per voxel or increasing the voxel/leaf size.\n", this->
getClassName ().c_str ());
291 PCL_WARN (
"[%s::filter] No voxels with a sufficient number of points. Grid will not be searchable. You can try reducing the min number of points required per voxel or increasing the voxel/leaf size\n", this->
getClassName ().c_str ());
307 auto leaf_iter =
leaves_.find (index);
308 if (leaf_iter !=
leaves_.end ())
332 auto leaf_iter =
leaves_.find (idx);
333 if (leaf_iter !=
leaves_.end ())
358 auto leaf_iter =
leaves_.find (idx);
359 if (leaf_iter !=
leaves_.end ())
377 getNeighborhoodAtPoint (
const Eigen::Matrix<int, 3, Eigen::Dynamic>& relative_coordinates,
const PointT& reference_point, std::vector<LeafConstPtr> &neighbors)
const;
418 inline const std::map<std::size_t, Leaf>&
452 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
const
459 PCL_WARN (
"[%s::nearestKSearch] Not Searchable\n", this->
getClassName ().c_str ());
465 k =
kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
468 k_leaves.reserve (k);
469 for (
const auto &k_index : k_indices)
476 k_leaves.push_back(&voxel->second);
478 return k_leaves.size();
492 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
const
494 if (index >=
static_cast<int> (cloud.size ()) || index < 0)
496 return (
nearestKSearch (cloud[index], k, k_leaves, k_sqr_distances));
511 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
const
518 PCL_WARN (
"[%s::radiusSearch] Not Searchable\n", this->
getClassName ().c_str ());
524 const int k =
kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
527 k_leaves.reserve (k);
528 for (
const auto &k_index : k_indices)
535 k_leaves.push_back(&voxel->second);
537 return k_leaves.size();
552 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
553 unsigned int max_nn = 0)
const
555 if (index >=
static_cast<int> (cloud.size ()) || index < 0)
557 return (
radiusSearch (cloud[index], radius, k_leaves, k_sqr_distances, max_nn));
590#ifdef PCL_NO_PRECOMPILE
591#include <pcl/filters/impl/voxel_grid_covariance.hpp>
const std::string & getClassName() const
Get a string representation of the name of this class.
std::string filter_name_
The filter name.
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
PointCloudConstPtr input_
The input point cloud dataset.
IndicesPtr indices_
A pointer to the vector of point indices to use.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
A searchable voxel structure containing the mean and covariance of the data.
void applyFilter(PointCloud &output) override
Filter cloud and initializes voxel structure.
shared_ptr< VoxelGrid< PointT > > Ptr
double getCovEigValueInflationRatio()
Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
LeafConstPtr getLeaf(int index)
Get the voxel containing point p.
int min_points_per_voxel_
Minimum points contained with in a voxel to allow it to be usable.
typename Filter< PointT >::PointCloud PointCloud
int nearestKSearch(const PointT &point, int k, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances) const
Search for the k-nearest occupied voxels for the given query point.
PointCloudPtr voxel_centroids_
Point cloud containing centroids of voxels containing atleast minimum number of points.
shared_ptr< const VoxelGrid< PointT > > ConstPtr
typename PointCloud::Ptr PointCloudPtr
void getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud, int pnt_per_cell=1000) const
Get a cloud to visualize each voxels normal distribution.
void setMinPointPerVoxel(int min_points_per_voxel)
Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance ...
int getAllNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get all 3x3x3 neighbor voxels of p (up to 27 voxels).
const std::map< std::size_t, Leaf > & getLeaves()
Get the leaf structure map.
LeafConstPtr getLeaf(Eigen::Vector3f &p)
Get the voxel containing point p.
void filter(PointCloud &output, bool searchable=false)
Filter cloud and initializes voxel structure.
int getVoxelAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p.
bool searchable_
Flag to determine if voxel structure is searchable.
double min_covar_eigvalue_mult_
Minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
KdTreeFLANN< PointT > kdtree_
KdTree generated using voxel_centroids_ (used for searching).
int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest occupied voxels of the query point in a given radius.
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
typename PointCloud::ConstPtr PointCloudConstPtr
void filter(bool searchable=false)
Initializes voxel structure.
int getNeighborhoodAtPoint(const Eigen::Matrix< int, 3, Eigen::Dynamic > &relative_coordinates, const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxels surrounding point p designated by relative_coordinates.
typename pcl::traits::fieldList< PointT >::type FieldList
VoxelGridCovariance()
Constructor.
void setCovEigValueInflationRatio(double min_covar_eigvalue_mult)
Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
int getMinPointPerVoxel()
Get the minimum number of points required for a cell to be used.
int nearestKSearch(const PointCloud &cloud, int index, int k, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances) const
Search for the k-nearest occupied voxels for the given query point.
std::vector< int > voxel_centroids_leaf_indices_
Indices of leaf structures associated with each point in voxel_centroids_ (used for searching).
PointCloudPtr getCentroids()
Get a pointcloud containing the voxel centroids.
std::map< std::size_t, Leaf > leaves_
Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of poin...
int radiusSearch(const PointT &point, double radius, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest occupied voxels of the query point in a given radius.
int getFaceNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p and its facing voxels (up to 7 voxels).
LeafConstPtr getLeaf(PointT &p)
Get the voxel containing point p.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
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.
Eigen::Vector4i divb_mul_
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_).
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier.
double filter_limit_max_
The maximum allowed filter value a point will be considered from.
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::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Defines all the PCL implemented PointT point type structures.
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Simple structure to hold a centroid, covarince and the number of points in a leaf.
Eigen::Matrix3d getEvecs() const
Get the eigen vectors of the voxel covariance.
int nr_points
Number of points contained by voxel.
Eigen::Vector3d mean_
3D voxel centroid
Eigen::Matrix3d icov_
Inverse of voxel covariance matrix.
Eigen::Matrix3d getCov() const
Get the voxel covariance.
Eigen::Vector3d getEvals() const
Get the eigen values of the voxel covariance.
Eigen::VectorXf centroid
Nd voxel centroid.
Eigen::Matrix3d getInverseCov() const
Get the inverse of the voxel covariance.
Eigen::Vector3d evals_
Eigen values of voxel covariance matrix.
Eigen::Vector3d getMean() const
Get the voxel centroid.
int getPointCount() const
Get the number of points contained by this voxel.
Eigen::Matrix3d cov_
Voxel covariance matrix.
Eigen::Matrix3d evecs_
Eigen vectors of voxel covariance matrix.