Point Cloud Library (PCL)  1.9.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 #pragma once
39 
40 #include <pcl/filters/boost.h>
41 #include <pcl/filters/voxel_grid.h>
42 #include <map>
43 #include <pcl/point_types.h>
44 #include <pcl/kdtree/kdtree_flann.h>
45 
46 namespace pcl
47 {
48  /** \brief A searchable voxel strucure containing the mean and covariance of the data.
49  * \note For more information please see
50  * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
51  * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
52  * PhD thesis, Orebro University. Orebro Studies in Technology 36</b>
53  * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
54  */
55  template<typename PointT>
56  class VoxelGridCovariance : public VoxelGrid<PointT>
57  {
58  protected:
67 
77 
78 
81  typedef typename PointCloud::Ptr PointCloudPtr;
83 
84  public:
85 
86  typedef boost::shared_ptr< VoxelGrid<PointT> > Ptr;
87  typedef boost::shared_ptr< const VoxelGrid<PointT> > ConstPtr;
88 
89 
90  /** \brief Simple structure to hold a centroid, covarince and the number of points in a leaf.
91  * Inverse covariance, eigen vectors and engen values are precomputed. */
92  struct Leaf
93  {
94  /** \brief Constructor.
95  * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix
96  */
97  Leaf () :
98  nr_points (0),
99  mean_ (Eigen::Vector3d::Zero ()),
100  centroid (),
101  cov_ (Eigen::Matrix3d::Identity ()),
102  icov_ (Eigen::Matrix3d::Zero ()),
103  evecs_ (Eigen::Matrix3d::Identity ()),
104  evals_ (Eigen::Vector3d::Zero ())
105  {
106  }
107 
108  /** \brief Get the voxel covariance.
109  * \return covariance matrix
110  */
111  Eigen::Matrix3d
112  getCov () const
113  {
114  return (cov_);
115  }
116 
117  /** \brief Get the inverse of the voxel covariance.
118  * \return inverse covariance matrix
119  */
120  Eigen::Matrix3d
121  getInverseCov () const
122  {
123  return (icov_);
124  }
125 
126  /** \brief Get the voxel centroid.
127  * \return centroid
128  */
129  Eigen::Vector3d
130  getMean () const
131  {
132  return (mean_);
133  }
134 
135  /** \brief Get the eigen vectors of the voxel covariance.
136  * \note Order corresponds with \ref getEvals
137  * \return matrix whose columns contain eigen vectors
138  */
139  Eigen::Matrix3d
140  getEvecs () const
141  {
142  return (evecs_);
143  }
144 
145  /** \brief Get the eigen values of the voxel covariance.
146  * \note Order corresponds with \ref getEvecs
147  * \return vector of eigen values
148  */
149  Eigen::Vector3d
150  getEvals () const
151  {
152  return (evals_);
153  }
154 
155  /** \brief Get the number of points contained by this voxel.
156  * \return number of points
157  */
158  int
159  getPointCount () const
160  {
161  return (nr_points);
162  }
163 
164  /** \brief Number of points contained by voxel */
166 
167  /** \brief 3D voxel centroid */
168  Eigen::Vector3d mean_;
169 
170  /** \brief Nd voxel centroid
171  * \note Differs from \ref mean_ when color data is used
172  */
173  Eigen::VectorXf centroid;
174 
175  /** \brief Voxel covariance matrix */
176  Eigen::Matrix3d cov_;
177 
178  /** \brief Inverse of voxel covariance matrix */
179  Eigen::Matrix3d icov_;
180 
181  /** \brief Eigen vectors of voxel covariance matrix */
182  Eigen::Matrix3d evecs_;
183 
184  /** \brief Eigen values of voxel covariance matrix */
185  Eigen::Vector3d evals_;
186 
187  };
188 
189  /** \brief Pointer to VoxelGridCovariance leaf structure */
190  typedef Leaf* LeafPtr;
191 
192  /** \brief Const pointer to VoxelGridCovariance leaf structure */
193  typedef const Leaf* LeafConstPtr;
194 
195  public:
196 
197  /** \brief Constructor.
198  * Sets \ref leaf_size_ to 0 and \ref searchable_ to false.
199  */
201  searchable_ (true),
204  leaves_ (),
205  voxel_centroids_ (),
207  kdtree_ ()
208  {
209  downsample_all_data_ = false;
210  save_leaf_layout_ = false;
211  leaf_size_.setZero ();
212  min_b_.setZero ();
213  max_b_.setZero ();
214  filter_name_ = "VoxelGridCovariance";
215  }
216 
217  /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation).
218  * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
219  */
220  inline void
221  setMinPointPerVoxel (int min_points_per_voxel)
222  {
223  if(min_points_per_voxel > 2)
224  {
225  min_points_per_voxel_ = min_points_per_voxel;
226  }
227  else
228  {
229  PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ());
231  }
232  }
233 
234  /** \brief Get the minimum number of points required for a cell to be used.
235  * \return the minimum number of points for required for a voxel to be used
236  */
237  inline int
239  {
240  return min_points_per_voxel_;
241  }
242 
243  /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
244  * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues
245  */
246  inline void
247  setCovEigValueInflationRatio (double min_covar_eigvalue_mult)
248  {
249  min_covar_eigvalue_mult_ = min_covar_eigvalue_mult;
250  }
251 
252  /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
253  * \return the minimum allowable ratio between eigenvalues
254  */
255  inline double
257  {
259  }
260 
261  /** \brief Filter cloud and initializes voxel structure.
262  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
263  * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
264  */
265  inline void
266  filter (PointCloud &output, bool searchable = false)
267  {
268  searchable_ = searchable;
269  applyFilter (output);
270 
271  voxel_centroids_ = PointCloudPtr (new PointCloud (output));
272 
273  if (searchable_ && voxel_centroids_->size() > 0)
274  {
275  // Initiates kdtree of the centroids of voxels containing a sufficient number of points
277  }
278  }
279 
280  /** \brief Initializes voxel structure.
281  * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
282  */
283  inline void
284  filter (bool searchable = false)
285  {
286  searchable_ = searchable;
287  voxel_centroids_ = PointCloudPtr (new PointCloud);
289 
290  if (searchable_ && voxel_centroids_->size() > 0)
291  {
292  // Initiates kdtree of the centroids of voxels containing a sufficient number of points
294  }
295  }
296 
297  /** \brief Get the voxel containing point p.
298  * \param[in] index the index of the leaf structure node
299  * \return const pointer to leaf structure
300  */
301  inline LeafConstPtr
302  getLeaf (int index)
303  {
304  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (index);
305  if (leaf_iter != leaves_.end ())
306  {
307  LeafConstPtr ret (&(leaf_iter->second));
308  return ret;
309  }
310  else
311  return NULL;
312  }
313 
314  /** \brief Get the voxel containing point p.
315  * \param[in] p the point to get the leaf structure at
316  * \return const pointer to leaf structure
317  */
318  inline LeafConstPtr
320  {
321  // Generate index associated with p
322  int ijk0 = static_cast<int> (floor (p.x * inverse_leaf_size_[0]) - min_b_[0]);
323  int ijk1 = static_cast<int> (floor (p.y * inverse_leaf_size_[1]) - min_b_[1]);
324  int ijk2 = static_cast<int> (floor (p.z * inverse_leaf_size_[2]) - min_b_[2]);
325 
326  // Compute the centroid leaf index
327  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
328 
329  // Find leaf associated with index
330  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (idx);
331  if (leaf_iter != leaves_.end ())
332  {
333  // If such a leaf exists return the pointer to the leaf structure
334  LeafConstPtr ret (&(leaf_iter->second));
335  return ret;
336  }
337  else
338  return NULL;
339  }
340 
341  /** \brief Get the voxel containing point p.
342  * \param[in] p the point to get the leaf structure at
343  * \return const pointer to leaf structure
344  */
345  inline LeafConstPtr
346  getLeaf (Eigen::Vector3f &p)
347  {
348  // Generate index associated with p
349  int ijk0 = static_cast<int> (floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]);
350  int ijk1 = static_cast<int> (floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]);
351  int ijk2 = static_cast<int> (floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]);
352 
353  // Compute the centroid leaf index
354  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
355 
356  // Find leaf associated with index
357  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (idx);
358  if (leaf_iter != leaves_.end ())
359  {
360  // If such a leaf exists return the pointer to the leaf structure
361  LeafConstPtr ret (&(leaf_iter->second));
362  return ret;
363  }
364  else
365  return NULL;
366 
367  }
368 
369  /** \brief Get the voxels surrounding point p, not including the voxel containing point p.
370  * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice).
371  * \param[in] reference_point the point to get the leaf structure at
372  * \param[out] neighbors
373  * \return number of neighbors found
374  */
375  int
376  getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors);
377 
378  /** \brief Get the leaf structure map
379  * \return a map contataining all leaves
380  */
381  inline const std::map<size_t, Leaf>&
383  {
384  return leaves_;
385  }
386 
387  /** \brief Get a pointcloud containing the voxel centroids
388  * \note Only voxels containing a sufficient number of points are used.
389  * \return a map contataining all leaves
390  */
391  inline PointCloudPtr
393  {
394  return voxel_centroids_;
395  }
396 
397 
398  /** \brief Get a cloud to visualize each voxels normal distribution.
399  * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
400  */
401  void
403 
404  /** \brief Search for the k-nearest occupied voxels for the given query point.
405  * \note Only voxels containing a sufficient number of points are used.
406  * \param[in] point the given query point
407  * \param[in] k the number of neighbors to search for
408  * \param[out] k_leaves the resultant leaves of the neighboring points
409  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
410  * \return number of neighbors found
411  */
412  int
413  nearestKSearch (const PointT &point, int k,
414  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
415  {
416  k_leaves.clear ();
417 
418  // Check if kdtree has been built
419  if (!searchable_)
420  {
421  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
422  return 0;
423  }
424 
425  // Find k-nearest neighbors in the occupied voxel centroid cloud
426  std::vector<int> k_indices;
427  k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
428 
429  // Find leaves corresponding to neighbors
430  k_leaves.reserve (k);
431  for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
432  {
433  k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]);
434  }
435  return k;
436  }
437 
438  /** \brief Search for the k-nearest occupied voxels for the given query point.
439  * \note Only voxels containing a sufficient number of points are used.
440  * \param[in] cloud the given query point
441  * \param[in] index the index
442  * \param[in] k the number of neighbors to search for
443  * \param[out] k_leaves the resultant leaves of the neighboring points
444  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
445  * \return number of neighbors found
446  */
447  inline int
448  nearestKSearch (const PointCloud &cloud, int index, int k,
449  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
450  {
451  if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
452  return (0);
453  return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances));
454  }
455 
456 
457  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
458  * \note Only voxels containing a sufficient number of points are used.
459  * \param[in] point the given query point
460  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
461  * \param[out] k_leaves the resultant leaves of the neighboring points
462  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
463  * \param[in] max_nn
464  * \return number of neighbors found
465  */
466  int
467  radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
468  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
469  {
470  k_leaves.clear ();
471 
472  // Check if kdtree has been built
473  if (!searchable_)
474  {
475  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
476  return 0;
477  }
478 
479  // Find neighbors within radius in the occupied voxel centroid cloud
480  std::vector<int> k_indices;
481  int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
482 
483  // Find leaves corresponding to neighbors
484  k_leaves.reserve (k);
485  for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
486  {
487  k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]);
488  }
489  return k;
490  }
491 
492  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
493  * \note Only voxels containing a sufficient number of points are used.
494  * \param[in] cloud the given query point
495  * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
496  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
497  * \param[out] k_leaves the resultant leaves of the neighboring points
498  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
499  * \param[in] max_nn
500  * \return number of neighbors found
501  */
502  inline int
503  radiusSearch (const PointCloud &cloud, int index, double radius,
504  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
505  unsigned int max_nn = 0)
506  {
507  if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
508  return (0);
509  return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn));
510  }
511 
512  protected:
513 
514  /** \brief Filter cloud and initializes voxel structure.
515  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
516  */
517  void applyFilter (PointCloud &output);
518 
519  /** \brief Flag to determine if voxel structure is searchable. */
521 
522  /** \brief Minimum points contained with in a voxel to allow it to be usable. */
524 
525  /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
527 
528  /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
529  std::map<size_t, Leaf> leaves_;
530 
531  /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */
532  PointCloudPtr voxel_centroids_;
533 
534  /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */
536 
537  /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
539  };
540 }
541 
542 #ifdef PCL_NO_PRECOMPILE
543 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
544 #endif
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.
void filter(PointCloud &output, bool searchable=false)
Filter cloud and initializes voxel structure.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:409
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
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
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:177
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.
Definition: bfgs.h:9
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
Definition: point_cloud.h:427
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.
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 usable.
Eigen::Matrix3d evecs_
Eigen vectors of voxel covariance matrix.
boost::shared_ptr< const PointCloud< PointTarget > > ConstPtr
Definition: point_cloud.h:428
Eigen::Vector4i divb_mul_
Definition: voxel_grid.h:472
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
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:165
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:181
int getPointCount() const
Get the number of points contained by this voxel.