38 #ifndef PCL_VOXEL_GRID_COVARIANCE_H_ 39 #define PCL_VOXEL_GRID_COVARIANCE_H_ 41 #include <pcl/filters/boost.h> 42 #include <pcl/filters/voxel_grid.h> 45 #include <pcl/kdtree/kdtree_flann.h> 56 template<
typename Po
intT>
87 typedef boost::shared_ptr< VoxelGrid<PointT> >
Ptr;
88 typedef boost::shared_ptr< const VoxelGrid<PointT> >
ConstPtr;
224 if(min_points_per_voxel > 2)
230 PCL_WARN (
"%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->
getClassName ().c_str ());
267 filter (PointCloud &output,
bool searchable =
false)
305 typename std::map<size_t, Leaf>::iterator leaf_iter =
leaves_.find (index);
306 if (leaf_iter !=
leaves_.end ())
308 LeafConstPtr ret (&(leaf_iter->second));
331 typename std::map<size_t, Leaf>::iterator leaf_iter =
leaves_.find (idx);
332 if (leaf_iter !=
leaves_.end ())
335 LeafConstPtr ret (&(leaf_iter->second));
351 int ijk1 =
static_cast<int> (floor (p[1] * inverse_leaf_size_[1]) -
min_b_[1]);
352 int ijk2 =
static_cast<int> (floor (p[2] * inverse_leaf_size_[2]) -
min_b_[2]);
358 typename std::map<size_t, Leaf>::iterator leaf_iter =
leaves_.find (idx);
359 if (leaf_iter !=
leaves_.end ())
362 LeafConstPtr ret (&(leaf_iter->second));
382 inline const std::map<size_t, Leaf>&
415 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
422 PCL_WARN (
"%s: Not Searchable", this->
getClassName ().c_str ());
427 std::vector<int> k_indices;
431 k_leaves.reserve (k);
432 for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
450 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
452 if (index >= static_cast<int> (cloud.
points.size ()) || index < 0)
469 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
476 PCL_WARN (
"%s: Not Searchable", this->
getClassName ().c_str ());
481 std::vector<int> k_indices;
485 k_leaves.reserve (k);
486 for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
505 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
506 unsigned int max_nn = 0)
508 if (index >= static_cast<int> (cloud.
points.size ()) || index < 0)
510 return (
radiusSearch (cloud.
points[index], radius, k_leaves, k_sqr_distances, max_nn));
543 #ifdef PCL_NO_PRECOMPILE 544 #include <pcl/filters/impl/voxel_grid_covariance.hpp> 547 #endif //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_ Eigen::Matrix3d getEvecs() const
Get the eigen vectors of the voxel covariance.
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.
LeafConstPtr getLeaf(Eigen::Vector3f &p)
Get the voxel containing point p.
void filter(PointCloud &output, bool searchable=false)
Filter cloud and initializes voxel structure.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
PointCloudPtr voxel_centroids_
Point cloud containing centroids of voxels containing atleast minimum number of points.
LeafConstPtr getLeaf(PointT &p)
Get the voxel containing point p.
LeafConstPtr getLeaf(int index)
Get the voxel containing point p.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
This file defines compatibility wrappers for low level I/O functions.
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.
Eigen::Vector3d mean_
3D voxel centroid
Eigen::Matrix3d icov_
Inverse of voxel covariance matrix.
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
double min_covar_eigvalue_mult_
Minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
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.
PointCloud::ConstPtr PointCloudConstPtr
void filter(bool searchable=false)
Initializes voxel structure.
int getMinPointPerVoxel()
Get the minimum number of points required for a cell to be used.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Leaf * LeafPtr
Pointer to VoxelGridCovariance leaf structure.
int getNeighborhoodAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors)
Get the voxels surrounding point p, not including the voxel containing point p.
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query point in a given radius.
std::vector< int > voxel_centroids_leaf_indices_
Indices of leaf structurs associated with each point in voxel_centroids_ (used for searching)...
Simple structure to hold a centroid, covarince and the number of points in a leaf.
boost::shared_ptr< PointCloud< PointT > > Ptr
void getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud)
Get a cloud to visualize each voxels normal distribution.
double getCovEigValueInflationRatio()
Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices...
bool searchable_
Flag to determine if voxel structure is searchable.
Eigen::VectorXf centroid
Nd voxel centroid.
VoxelGridCovariance()
Constructor.
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.
Eigen::Matrix3d getCov() const
Get the voxel covariance.
pcl::traits::fieldList< PointT >::type FieldList
void applyFilter(PointCloud &output)
Filter cloud and initializes voxel structure.
PointCloudPtr getCentroids()
Get a pointcloud containing the voxel centroids.
const std::map< size_t, Leaf > & getLeaves()
Get the leaf structure map.
int min_points_per_voxel_
Minimum points contained with in a voxel to allow it to be usable.
Eigen::Matrix3d evecs_
Eigen vectors of voxel covariance matrix.
boost::shared_ptr< const PointCloud< PointTarget > > ConstPtr
Eigen::Vector4i divb_mul_
PointCloud::Ptr PointCloudPtr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
boost::shared_ptr< VoxelGrid< PointT > > Ptr
void setCovEigValueInflationRatio(double min_covar_eigvalue_mult)
Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices...
boost::shared_ptr< const VoxelGrid< PointT > > ConstPtr
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point.
Eigen::Vector4f leaf_size_
The size of a leaf.
Eigen::Vector3d getEvals() const
Get the eigen values of the voxel covariance.
Eigen::Vector3d getMean() const
Get the voxel centroid.
int nr_points
Number of points contained by voxel.
Eigen::Vector3d evals_
Eigen values of voxel covariance matrix.
const std::string & getClassName() const
Get a string representation of the name of this class.
Eigen::Matrix3d getInverseCov() const
Get the inverse of the voxel covariance.
KdTreeFLANN< PointT > kdtree_
KdTree generated using voxel_centroids_ (used for searching).
std::string filter_name_
The filter name.
Filter< PointT >::PointCloud PointCloud
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.
int getPointCount() const
Get the number of points contained by this voxel.
std::map< size_t, Leaf > leaves_
Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of poin...
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 ...
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
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.
A searchable voxel strucure containing the mean and covariance of the data.
Eigen::Matrix3d cov_
Voxel covariance matrix.