Point Cloud Library (PCL)  1.8.1-dev
voxel_grid_covariance.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_VOXEL_GRID_COVARIANCE_H_
39 #define PCL_VOXEL_GRID_COVARIANCE_H_
40 
41 #include <pcl/filters/boost.h>
42 #include <pcl/filters/voxel_grid.h>
43 #include <map>
44 #include <pcl/point_types.h>
45 #include <pcl/kdtree/kdtree_flann.h>
46 
47 namespace pcl
48 {
49  /** \brief A searchable voxel strucure containing the mean and covariance of the data.
50  * \note For more information please see
51  * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
52  * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
53  * PhD thesis, Orebro University. Orebro Studies in Technology 36</b>
54  * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
55  */
56  template<typename PointT>
57  class VoxelGridCovariance : public VoxelGrid<PointT>
58  {
59  protected:
68 
78 
79 
82  typedef typename PointCloud::Ptr PointCloudPtr;
84 
85  public:
86 
87  typedef boost::shared_ptr< VoxelGrid<PointT> > Ptr;
88  typedef boost::shared_ptr< const VoxelGrid<PointT> > ConstPtr;
89 
90 
91  /** \brief Simple structure to hold a centroid, covarince and the number of points in a leaf.
92  * Inverse covariance, eigen vectors and engen values are precomputed. */
93  struct Leaf
94  {
95  /** \brief Constructor.
96  * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix
97  */
98  Leaf () :
99  nr_points (0),
100  mean_ (Eigen::Vector3d::Zero ()),
101  centroid (),
102  cov_ (Eigen::Matrix3d::Identity ()),
103  icov_ (Eigen::Matrix3d::Zero ()),
104  evecs_ (Eigen::Matrix3d::Identity ()),
105  evals_ (Eigen::Vector3d::Zero ())
106  {
107  }
108 
109  /** \brief Get the voxel covariance.
110  * \return covariance matrix
111  */
112  Eigen::Matrix3d
113  getCov () const
114  {
115  return (cov_);
116  }
117 
118  /** \brief Get the inverse of the voxel covariance.
119  * \return inverse covariance matrix
120  */
121  Eigen::Matrix3d
122  getInverseCov () const
123  {
124  return (icov_);
125  }
126 
127  /** \brief Get the voxel centroid.
128  * \return centroid
129  */
130  Eigen::Vector3d
131  getMean () const
132  {
133  return (mean_);
134  }
135 
136  /** \brief Get the eigen vectors of the voxel covariance.
137  * \note Order corresponds with \ref getEvals
138  * \return matrix whose columns contain eigen vectors
139  */
140  Eigen::Matrix3d
141  getEvecs () const
142  {
143  return (evecs_);
144  }
145 
146  /** \brief Get the eigen values of the voxel covariance.
147  * \note Order corresponds with \ref getEvecs
148  * \return vector of eigen values
149  */
150  Eigen::Vector3d
151  getEvals () const
152  {
153  return (evals_);
154  }
155 
156  /** \brief Get the number of points contained by this voxel.
157  * \return number of points
158  */
159  int
160  getPointCount () const
161  {
162  return (nr_points);
163  }
164 
165  /** \brief Number of points contained by voxel */
167 
168  /** \brief 3D voxel centroid */
169  Eigen::Vector3d mean_;
170 
171  /** \brief Nd voxel centroid
172  * \note Differs from \ref mean_ when color data is used
173  */
174  Eigen::VectorXf centroid;
175 
176  /** \brief Voxel covariance matrix */
177  Eigen::Matrix3d cov_;
178 
179  /** \brief Inverse of voxel covariance matrix */
180  Eigen::Matrix3d icov_;
181 
182  /** \brief Eigen vectors of voxel covariance matrix */
183  Eigen::Matrix3d evecs_;
184 
185  /** \brief Eigen values of voxel covariance matrix */
186  Eigen::Vector3d evals_;
187 
188  };
189 
190  /** \brief Pointer to VoxelGridCovariance leaf structure */
191  typedef Leaf* LeafPtr;
192 
193  /** \brief Const pointer to VoxelGridCovariance leaf structure */
194  typedef const Leaf* LeafConstPtr;
195 
196  public:
197 
198  /** \brief Constructor.
199  * Sets \ref leaf_size_ to 0 and \ref searchable_ to false.
200  */
202  searchable_ (true),
205  leaves_ (),
206  voxel_centroids_ (),
208  kdtree_ ()
209  {
210  downsample_all_data_ = false;
211  save_leaf_layout_ = false;
212  leaf_size_.setZero ();
213  min_b_.setZero ();
214  max_b_.setZero ();
215  filter_name_ = "VoxelGridCovariance";
216  }
217 
218  /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation).
219  * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
220  */
221  inline void
222  setMinPointPerVoxel (int min_points_per_voxel)
223  {
224  if(min_points_per_voxel > 2)
225  {
226  min_points_per_voxel_ = min_points_per_voxel;
227  }
228  else
229  {
230  PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ());
232  }
233  }
234 
235  /** \brief Get the minimum number of points required for a cell to be used.
236  * \return the minimum number of points for required for a voxel to be used
237  */
238  inline int
240  {
241  return min_points_per_voxel_;
242  }
243 
244  /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
245  * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues
246  */
247  inline void
248  setCovEigValueInflationRatio (double min_covar_eigvalue_mult)
249  {
250  min_covar_eigvalue_mult_ = min_covar_eigvalue_mult;
251  }
252 
253  /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
254  * \return the minimum allowable ratio between eigenvalues
255  */
256  inline double
258  {
260  }
261 
262  /** \brief Filter cloud and initializes voxel structure.
263  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
264  * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
265  */
266  inline void
267  filter (PointCloud &output, bool searchable = false)
268  {
269  searchable_ = searchable;
270  applyFilter (output);
271 
272  voxel_centroids_ = PointCloudPtr (new PointCloud (output));
273 
274  if (searchable_ && voxel_centroids_->size() > 0)
275  {
276  // Initiates kdtree of the centroids of voxels containing a sufficient number of points
278  }
279  }
280 
281  /** \brief Initializes voxel structure.
282  * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
283  */
284  inline void
285  filter (bool searchable = false)
286  {
287  searchable_ = searchable;
290 
291  if (searchable_ && voxel_centroids_->size() > 0)
292  {
293  // Initiates kdtree of the centroids of voxels containing a sufficient number of points
295  }
296  }
297 
298  /** \brief Get the voxel containing point p.
299  * \param[in] index the index of the leaf structure node
300  * \return const pointer to leaf structure
301  */
302  inline LeafConstPtr
303  getLeaf (int index)
304  {
305  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (index);
306  if (leaf_iter != leaves_.end ())
307  {
308  LeafConstPtr ret (&(leaf_iter->second));
309  return ret;
310  }
311  else
312  return NULL;
313  }
314 
315  /** \brief Get the voxel containing point p.
316  * \param[in] p the point to get the leaf structure at
317  * \return const pointer to leaf structure
318  */
319  inline LeafConstPtr
321  {
322  // Generate index associated with p
323  int ijk0 = static_cast<int> (floor (p.x * inverse_leaf_size_[0]) - min_b_[0]);
324  int ijk1 = static_cast<int> (floor (p.y * inverse_leaf_size_[1]) - min_b_[1]);
325  int ijk2 = static_cast<int> (floor (p.z * inverse_leaf_size_[2]) - min_b_[2]);
326 
327  // Compute the centroid leaf index
328  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
329 
330  // Find leaf associated with index
331  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (idx);
332  if (leaf_iter != leaves_.end ())
333  {
334  // If such a leaf exists return the pointer to the leaf structure
335  LeafConstPtr ret (&(leaf_iter->second));
336  return ret;
337  }
338  else
339  return NULL;
340  }
341 
342  /** \brief Get the voxel containing point p.
343  * \param[in] p the point to get the leaf structure at
344  * \return const pointer to leaf structure
345  */
346  inline LeafConstPtr
347  getLeaf (Eigen::Vector3f &p)
348  {
349  // Generate index associated with p
350  int ijk0 = static_cast<int> (floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]);
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]);
353 
354  // Compute the centroid leaf index
355  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
356 
357  // Find leaf associated with index
358  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (idx);
359  if (leaf_iter != leaves_.end ())
360  {
361  // If such a leaf exists return the pointer to the leaf structure
362  LeafConstPtr ret (&(leaf_iter->second));
363  return ret;
364  }
365  else
366  return NULL;
367 
368  }
369 
370  /** \brief Get the voxels surrounding point p, not including the voxel contating point p.
371  * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice).
372  * \param[in] reference_point the point to get the leaf structure at
373  * \param[out] neighbors
374  * \return number of neighbors found
375  */
376  int
377  getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors);
378 
379  /** \brief Get the leaf structure map
380  * \return a map contataining all leaves
381  */
382  inline const std::map<size_t, Leaf>&
384  {
385  return leaves_;
386  }
387 
388  /** \brief Get a pointcloud containing the voxel centroids
389  * \note Only voxels containing a sufficient number of points are used.
390  * \return a map contataining all leaves
391  */
392  inline PointCloudPtr
394  {
395  return voxel_centroids_;
396  }
397 
398 
399  /** \brief Get a cloud to visualize each voxels normal distribution.
400  * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
401  */
402  void
404 
405  /** \brief Search for the k-nearest occupied voxels for the given query point.
406  * \note Only voxels containing a sufficient number of points are used.
407  * \param[in] point the given query point
408  * \param[in] k the number of neighbors to search for
409  * \param[out] k_leaves the resultant leaves of the neighboring points
410  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
411  * \return number of neighbors found
412  */
413  int
414  nearestKSearch (const PointT &point, int k,
415  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
416  {
417  k_leaves.clear ();
418 
419  // Check if kdtree has been built
420  if (!searchable_)
421  {
422  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
423  return 0;
424  }
425 
426  // Find k-nearest neighbors in the occupied voxel centroid cloud
427  std::vector<int> k_indices;
428  k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
429 
430  // Find leaves corresponding to neighbors
431  k_leaves.reserve (k);
432  for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
433  {
434  k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]);
435  }
436  return k;
437  }
438 
439  /** \brief Search for the k-nearest occupied voxels for the given query point.
440  * \note Only voxels containing a sufficient number of points are used.
441  * \param[in] cloud the given query point
442  * \param[in] index the index
443  * \param[in] k the number of neighbors to search for
444  * \param[out] k_leaves the resultant leaves of the neighboring points
445  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
446  * \return number of neighbors found
447  */
448  inline int
449  nearestKSearch (const PointCloud &cloud, int index, int k,
450  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
451  {
452  if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
453  return (0);
454  return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances));
455  }
456 
457 
458  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
459  * \note Only voxels containing a sufficient number of points are used.
460  * \param[in] point the given query point
461  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
462  * \param[out] k_leaves the resultant leaves of the neighboring points
463  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
464  * \param[in] max_nn
465  * \return number of neighbors found
466  */
467  int
468  radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
469  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
470  {
471  k_leaves.clear ();
472 
473  // Check if kdtree has been built
474  if (!searchable_)
475  {
476  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
477  return 0;
478  }
479 
480  // Find neighbors within radius in the occupied voxel centroid cloud
481  std::vector<int> k_indices;
482  int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
483 
484  // Find leaves corresponding to neighbors
485  k_leaves.reserve (k);
486  for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
487  {
488  k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]);
489  }
490  return k;
491  }
492 
493  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
494  * \note Only voxels containing a sufficient number of points are used.
495  * \param[in] cloud the given query point
496  * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
497  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
498  * \param[out] k_leaves the resultant leaves of the neighboring points
499  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
500  * \param[in] max_nn
501  * \return number of neighbors found
502  */
503  inline int
504  radiusSearch (const PointCloud &cloud, int index, double radius,
505  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
506  unsigned int max_nn = 0)
507  {
508  if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
509  return (0);
510  return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn));
511  }
512 
513  protected:
514 
515  /** \brief Filter cloud and initializes voxel structure.
516  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
517  */
518  void applyFilter (PointCloud &output);
519 
520  /** \brief Flag to determine if voxel structure is searchable. */
522 
523  /** \brief Minimum points contained with in a voxel to allow it to be useable. */
525 
526  /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
528 
529  /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
530  std::map<size_t, Leaf> leaves_;
531 
532  /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */
534 
535  /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */
537 
538  /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
540  };
541 }
542 
543 #ifdef PCL_NO_PRECOMPILE
544 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
545 #endif
546 
547 #endif //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_
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.
Eigen::Vector3d getMean() const
Get the voxel centroid.
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.
Eigen::Vector3d getEvals() const
Get the eigen values of the voxel covariance.
LeafConstPtr getLeaf(Eigen::Vector3f &p)
Get the voxel containing point p.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void filter(PointCloud &output, bool searchable=false)
Filter cloud and initializes voxel structure.
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.
Definition: voxel_grid.h:463
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
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 getEvecs() const
Get the eigen vectors of the voxel covariance.
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 icov_
Inverse of voxel covariance matrix.
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
Eigen::Vector4i max_b_
Definition: voxel_grid.h:472
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.
Eigen::Matrix3d getCov() const
Get the voxel covariance.
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...
Definition: voxel_grid.h:178
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 contating point p.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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.
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.
PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:72
Eigen::VectorXf centroid
Nd voxel centroid.
Eigen::Matrix3d getInverseCov() const
Get the inverse of the voxel covariance.
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition: voxel_grid.h:460
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 useable.
Eigen::Matrix3d evecs_
Eigen vectors of voxel covariance matrix.
Eigen::Vector4i divb_mul_
Definition: voxel_grid.h:472
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
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition: voxel_grid.h:457
int nr_points
Number of points contained by voxel.
Eigen::Vector3d evals_
Eigen values of voxel covariance matrix.
KdTreeFLANN< PointT > kdtree_
KdTree generated using voxel_centroids_ (used for searching).
std::string filter_name_
The filter name.
Definition: filter.h:166
Filter< PointT >::PointCloud PointCloud
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
Definition: voxel_grid.h:472
A point structure representing Euclidean xyz coordinates, and the RGB color.
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_.
Definition: voxel_grid.h:466
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.
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: filter.h:182
int getPointCount() const
Get the number of points contained by this voxel.