40 #include <pcl/filters/boost.h>
41 #include <pcl/filters/voxel_grid.h>
44 #include <pcl/kdtree/kdtree_flann.h>
55 template<
typename Po
intT>
86 using Ptr = shared_ptr<VoxelGrid<PointT> >;
87 using ConstPtr = shared_ptr<const VoxelGrid<PointT> >;
221 if(min_points_per_voxel > 2)
227 PCL_WARN (
"%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->
getClassName ().c_str ());
302 typename std::map<std::size_t, Leaf>::iterator leaf_iter =
leaves_.find (index);
303 if (leaf_iter !=
leaves_.end ())
327 typename std::map<std::size_t, Leaf>::iterator leaf_iter =
leaves_.find (idx);
328 if (leaf_iter !=
leaves_.end ())
353 typename std::map<std::size_t, Leaf>::iterator leaf_iter =
leaves_.find (idx);
354 if (leaf_iter !=
leaves_.end ())
376 inline const std::map<std::size_t, Leaf>&
409 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
416 PCL_WARN (
"%s: Not Searchable", this->
getClassName ().c_str ());
421 std::vector<int> k_indices;
422 k =
kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
425 k_leaves.reserve (k);
426 for (
const int &k_index : k_indices)
444 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
446 if (index >=
static_cast<int> (cloud.
points.size ()) || index < 0)
463 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
470 PCL_WARN (
"%s: Not Searchable", this->
getClassName ().c_str ());
475 std::vector<int> k_indices;
476 int k =
kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
479 k_leaves.reserve (k);
480 for (
const int &k_index : k_indices)
499 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
500 unsigned int max_nn = 0)
502 if (index >=
static_cast<int> (cloud.
points.size ()) || index < 0)
504 return (
radiusSearch (cloud.
points[index], radius, k_leaves, k_sqr_distances, max_nn));
537 #ifdef PCL_NO_PRECOMPILE
538 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
const std::string & getClassName() const
Get a string representation of the name of this class.
shared_ptr< Filter< PointT > > Ptr
shared_ptr< const Filter< PointT > > ConstPtr
std::string filter_name_
The filter name.
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
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
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
shared_ptr< const PointCloud< PointT > > ConstPtr
A searchable voxel strucure containing the mean and covariance of the data.
void applyFilter(PointCloud &output) override
Filter cloud and initializes voxel structure.
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.
int nearestKSearch(const PointCloud &cloud, int index, int k, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances)
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.
typename PointCloud::Ptr PointCloudPtr
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 radiusSearch(const PointT &point, double radius, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all the nearest occupied voxels of the query point in a given radius.
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 radiusSearch(const PointCloud &cloud, int index, double radius, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all the nearest occupied voxels of the query point in a given radius.
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).
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
int nearestKSearch(const PointT &point, int k, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances)
Search for the k-nearest occupied voxels for the given query point.
void filter(bool searchable=false)
Initializes voxel structure.
int getNeighborhoodAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors)
Get the voxels surrounding point p, not including the voxel containing point p.
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.
std::vector< int > voxel_centroids_leaf_indices_
Indices of leaf structurs associated with each point in voxel_centroids_ (used for searching).
void getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud)
Get a cloud to visualize each voxels normal distribution.
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...
const std::map< std::size_t, Leaf > & getLeaves()
Get the leaf structure map.
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.
Eigen::Vector4i divb_mul_
typename pcl::traits::fieldList< PointT >::type FieldList
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier.
Eigen::Vector4f leaf_size_
The size of a leaf.
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.
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.