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>
44 #include <pcl/point_types.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++)
449 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
451 if (index >= static_cast<int> (cloud.
points.size ()) || index < 0)
467 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
474 PCL_WARN (
"%s: Not Searchable", this->
getClassName ().c_str ());
479 std::vector<int> k_indices;
483 k_leaves.reserve (k);
484 for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
502 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
503 unsigned int max_nn = 0)
505 if (index >= static_cast<int> (cloud.
points.size ()) || index < 0)
507 return (
radiusSearch (cloud.
points[index], radius, k_leaves, k_sqr_distances, max_nn));
540 #ifdef PCL_NO_PRECOMPILE
541 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
544 #endif //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_
std::string filter_name_
The filter name.
Eigen::Vector3d mean_
3D voxel centroid
Simple structure to hold a centroid, covarince and the number of points in a leaf.
LeafConstPtr getLeaf(int index)
Get the voxel containing point p.
LeafConstPtr getLeaf(PointT &p)
Get the voxel containing point p.
PointCloudPtr voxel_centroids_
Point cloud containing centroids of voxels containing atleast minimum number of points.
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
Eigen::Matrix3d cov_
Voxel covariance matrix.
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Filter< PointT >::PointCloud PointCloud
const std::string & getClassName() const
Get a string representation of the name of this class.
pcl::traits::fieldList< PointT >::type FieldList
PointCloud::ConstPtr PointCloudConstPtr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void filter(bool searchable=false)
Initializes voxel structure.
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
boost::shared_ptr< VoxelGrid< PointT > > Ptr
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
double min_covar_eigvalue_mult_
Minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
const std::map< size_t, Leaf > & getLeaves()
Get the leaf structure map.
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.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
void filter(PointCloud &output, bool searchable=false)
Filter cloud and initializes voxel structure.
Eigen::Vector3d evals_
Eigen values of voxel covariance matrix.
int getNeighborhoodAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors)
Get the voxels surrounding point p, not including the voxel contating point p.
void applyFilter(PointCloud &output)
Filter cloud and initializes voxel structure.
Eigen::Matrix3d icov_
Inverse of voxel covariance matrix.
void setCovEigValueInflationRatio(double min_covar_eigvalue_mult)
Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices...
double getCovEigValueInflationRatio()
Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices...
PointCloudPtr getCentroids()
Get a pointcloud containing the voxel centroids.
Eigen::Vector4i divb_mul_
boost::shared_ptr< const PointCloud< PointTarget > > ConstPtr
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
VoxelGridCovariance()
Constructor.
int getPointCount() const
Get the number of points contained by this voxel.
int getMinPointPerVoxel()
Get the minimum number of points required for a cell to be used.
LeafConstPtr getLeaf(Eigen::Vector3f &p)
Get the voxel containing point p.
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.
Eigen::VectorXf centroid
Nd voxel centroid.
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)...
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 ...
Eigen::Matrix3d getCov() const
Get the voxel covariance.
Eigen::Vector4f leaf_size_
The size of a leaf.
bool searchable_
Flag to determine if voxel structure is searchable.
Eigen::Matrix3d evecs_
Eigen vectors of voxel covariance matrix.
Eigen::Vector3d getMean() const
Get the voxel centroid.
A searchable voxel strucure containing the mean and covariance of the data.
Eigen::Vector3d getEvals() const
Get the eigen values of the voxel covariance.
int min_points_per_voxel_
Minimum points contained with in a voxel to allow it to be useable.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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.
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.
Leaf * LeafPtr
Pointer to VoxelGridCovariance leaf structure.
std::map< size_t, Leaf > leaves_
Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of poin...
boost::shared_ptr< PointCloud< PointT > > Ptr
PointCloud::Ptr PointCloudPtr
void getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud)
Get a cloud to visualize each voxels normal distribution.
Eigen::Matrix3d getEvecs() const
Get the eigen vectors of the voxel covariance.
boost::shared_ptr< const VoxelGrid< PointT > > ConstPtr
KdTreeFLANN< PointT > kdtree_
KdTree generated using voxel_centroids_ (used for searching).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
int nr_points
Number of points contained by voxel.
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::Matrix3d getInverseCov() const
Get the inverse of the voxel covariance.