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  using PointCloudPtr = typename PointCloud::Ptr;
83 
84  public:
85 
86  using Ptr = boost::shared_ptr<VoxelGrid<PointT> >;
87  using ConstPtr = boost::shared_ptr<const VoxelGrid<PointT> >;
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  cov_ (Eigen::Matrix3d::Identity ()),
101  icov_ (Eigen::Matrix3d::Zero ()),
102  evecs_ (Eigen::Matrix3d::Identity ()),
103  evals_ (Eigen::Vector3d::Zero ())
104  {
105  }
106 
107  /** \brief Get the voxel covariance.
108  * \return covariance matrix
109  */
110  Eigen::Matrix3d
111  getCov () const
112  {
113  return (cov_);
114  }
115 
116  /** \brief Get the inverse of the voxel covariance.
117  * \return inverse covariance matrix
118  */
119  Eigen::Matrix3d
120  getInverseCov () const
121  {
122  return (icov_);
123  }
124 
125  /** \brief Get the voxel centroid.
126  * \return centroid
127  */
128  Eigen::Vector3d
129  getMean () const
130  {
131  return (mean_);
132  }
133 
134  /** \brief Get the eigen vectors of the voxel covariance.
135  * \note Order corresponds with \ref getEvals
136  * \return matrix whose columns contain eigen vectors
137  */
138  Eigen::Matrix3d
139  getEvecs () const
140  {
141  return (evecs_);
142  }
143 
144  /** \brief Get the eigen values of the voxel covariance.
145  * \note Order corresponds with \ref getEvecs
146  * \return vector of eigen values
147  */
148  Eigen::Vector3d
149  getEvals () const
150  {
151  return (evals_);
152  }
153 
154  /** \brief Get the number of points contained by this voxel.
155  * \return number of points
156  */
157  int
158  getPointCount () const
159  {
160  return (nr_points);
161  }
162 
163  /** \brief Number of points contained by voxel */
165 
166  /** \brief 3D voxel centroid */
167  Eigen::Vector3d mean_;
168 
169  /** \brief Nd voxel centroid
170  * \note Differs from \ref mean_ when color data is used
171  */
172  Eigen::VectorXf centroid;
173 
174  /** \brief Voxel covariance matrix */
175  Eigen::Matrix3d cov_;
176 
177  /** \brief Inverse of voxel covariance matrix */
178  Eigen::Matrix3d icov_;
179 
180  /** \brief Eigen vectors of voxel covariance matrix */
181  Eigen::Matrix3d evecs_;
182 
183  /** \brief Eigen values of voxel covariance matrix */
184  Eigen::Vector3d evals_;
185 
186  };
187 
188  /** \brief Pointer to VoxelGridCovariance leaf structure */
189  using LeafPtr = Leaf *;
190 
191  /** \brief Const pointer to VoxelGridCovariance leaf structure */
192  using LeafConstPtr = const Leaf *;
193 
194  public:
195 
196  /** \brief Constructor.
197  * Sets \ref leaf_size_ to 0 and \ref searchable_ to false.
198  */
200  searchable_ (true),
203  leaves_ (),
204  voxel_centroids_ (),
205  kdtree_ ()
206  {
207  downsample_all_data_ = false;
208  save_leaf_layout_ = false;
209  leaf_size_.setZero ();
210  min_b_.setZero ();
211  max_b_.setZero ();
212  filter_name_ = "VoxelGridCovariance";
213  }
214 
215  /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation).
216  * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
217  */
218  inline void
219  setMinPointPerVoxel (int min_points_per_voxel)
220  {
221  if(min_points_per_voxel > 2)
222  {
223  min_points_per_voxel_ = min_points_per_voxel;
224  }
225  else
226  {
227  PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ());
229  }
230  }
231 
232  /** \brief Get the minimum number of points required for a cell to be used.
233  * \return the minimum number of points for required for a voxel to be used
234  */
235  inline int
237  {
238  return min_points_per_voxel_;
239  }
240 
241  /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
242  * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues
243  */
244  inline void
245  setCovEigValueInflationRatio (double min_covar_eigvalue_mult)
246  {
247  min_covar_eigvalue_mult_ = min_covar_eigvalue_mult;
248  }
249 
250  /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
251  * \return the minimum allowable ratio between eigenvalues
252  */
253  inline double
255  {
257  }
258 
259  /** \brief Filter cloud and initializes voxel structure.
260  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
261  * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
262  */
263  inline void
264  filter (PointCloud &output, bool searchable = false)
265  {
266  searchable_ = searchable;
267  applyFilter (output);
268 
269  voxel_centroids_ = PointCloudPtr (new PointCloud (output));
270 
271  if (searchable_ && !voxel_centroids_->empty ())
272  {
273  // Initiates kdtree of the centroids of voxels containing a sufficient number of points
275  }
276  }
277 
278  /** \brief Initializes voxel structure.
279  * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
280  */
281  inline void
282  filter (bool searchable = false)
283  {
284  searchable_ = searchable;
287 
288  if (searchable_ && !voxel_centroids_->empty ())
289  {
290  // Initiates kdtree of the centroids of voxels containing a sufficient number of points
292  }
293  }
294 
295  /** \brief Get the voxel containing point p.
296  * \param[in] index the index of the leaf structure node
297  * \return const pointer to leaf structure
298  */
299  inline LeafConstPtr
300  getLeaf (int index)
301  {
302  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (index);
303  if (leaf_iter != leaves_.end ())
304  {
305  LeafConstPtr ret (&(leaf_iter->second));
306  return ret;
307  }
308  return nullptr;
309  }
310 
311  /** \brief Get the voxel containing point p.
312  * \param[in] p the point to get the leaf structure at
313  * \return const pointer to leaf structure
314  */
315  inline LeafConstPtr
317  {
318  // Generate index associated with p
319  int ijk0 = static_cast<int> (std::floor (p.x * inverse_leaf_size_[0]) - min_b_[0]);
320  int ijk1 = static_cast<int> (std::floor (p.y * inverse_leaf_size_[1]) - min_b_[1]);
321  int ijk2 = static_cast<int> (std::floor (p.z * inverse_leaf_size_[2]) - min_b_[2]);
322 
323  // Compute the centroid leaf index
324  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
325 
326  // Find leaf associated with index
327  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (idx);
328  if (leaf_iter != leaves_.end ())
329  {
330  // If such a leaf exists return the pointer to the leaf structure
331  LeafConstPtr ret (&(leaf_iter->second));
332  return ret;
333  }
334  return nullptr;
335  }
336 
337  /** \brief Get the voxel containing point p.
338  * \param[in] p the point to get the leaf structure at
339  * \return const pointer to leaf structure
340  */
341  inline LeafConstPtr
342  getLeaf (Eigen::Vector3f &p)
343  {
344  // Generate index associated with p
345  int ijk0 = static_cast<int> (std::floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]);
346  int ijk1 = static_cast<int> (std::floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]);
347  int ijk2 = static_cast<int> (std::floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]);
348 
349  // Compute the centroid leaf index
350  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
351 
352  // Find leaf associated with index
353  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (idx);
354  if (leaf_iter != leaves_.end ())
355  {
356  // If such a leaf exists return the pointer to the leaf structure
357  LeafConstPtr ret (&(leaf_iter->second));
358  return ret;
359  }
360  return nullptr;
361 
362  }
363 
364  /** \brief Get the voxels surrounding point p, not including the voxel containing point p.
365  * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice).
366  * \param[in] reference_point the point to get the leaf structure at
367  * \param[out] neighbors
368  * \return number of neighbors found
369  */
370  int
371  getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors);
372 
373  /** \brief Get the leaf structure map
374  * \return a map contataining all leaves
375  */
376  inline const std::map<size_t, Leaf>&
378  {
379  return leaves_;
380  }
381 
382  /** \brief Get a pointcloud containing the voxel centroids
383  * \note Only voxels containing a sufficient number of points are used.
384  * \return a map contataining all leaves
385  */
386  inline PointCloudPtr
388  {
389  return voxel_centroids_;
390  }
391 
392 
393  /** \brief Get a cloud to visualize each voxels normal distribution.
394  * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
395  */
396  void
398 
399  /** \brief Search for the k-nearest occupied voxels for the given query point.
400  * \note Only voxels containing a sufficient number of points are used.
401  * \param[in] point the given query point
402  * \param[in] k the number of neighbors to search for
403  * \param[out] k_leaves the resultant leaves of the neighboring points
404  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
405  * \return number of neighbors found
406  */
407  int
408  nearestKSearch (const PointT &point, int k,
409  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
410  {
411  k_leaves.clear ();
412 
413  // Check if kdtree has been built
414  if (!searchable_)
415  {
416  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
417  return 0;
418  }
419 
420  // Find k-nearest neighbors in the occupied voxel centroid cloud
421  std::vector<int> k_indices;
422  k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
423 
424  // Find leaves corresponding to neighbors
425  k_leaves.reserve (k);
426  for (const int &k_index : k_indices)
427  {
428  k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[k_index]]);
429  }
430  return k;
431  }
432 
433  /** \brief Search for the k-nearest occupied voxels for the given query point.
434  * \note Only voxels containing a sufficient number of points are used.
435  * \param[in] cloud the given query point
436  * \param[in] index the index
437  * \param[in] k the number of neighbors to search for
438  * \param[out] k_leaves the resultant leaves of the neighboring points
439  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
440  * \return number of neighbors found
441  */
442  inline int
443  nearestKSearch (const PointCloud &cloud, int index, int k,
444  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
445  {
446  if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
447  return (0);
448  return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances));
449  }
450 
451 
452  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
453  * \note Only voxels containing a sufficient number of points are used.
454  * \param[in] point the given query point
455  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
456  * \param[out] k_leaves the resultant leaves of the neighboring points
457  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
458  * \param[in] max_nn
459  * \return number of neighbors found
460  */
461  int
462  radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
463  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
464  {
465  k_leaves.clear ();
466 
467  // Check if kdtree has been built
468  if (!searchable_)
469  {
470  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
471  return 0;
472  }
473 
474  // Find neighbors within radius in the occupied voxel centroid cloud
475  std::vector<int> k_indices;
476  int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
477 
478  // Find leaves corresponding to neighbors
479  k_leaves.reserve (k);
480  for (const int &k_index : k_indices)
481  {
482  k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[k_index]]);
483  }
484  return k;
485  }
486 
487  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
488  * \note Only voxels containing a sufficient number of points are used.
489  * \param[in] cloud the given query point
490  * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
491  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
492  * \param[out] k_leaves the resultant leaves of the neighboring points
493  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
494  * \param[in] max_nn
495  * \return number of neighbors found
496  */
497  inline int
498  radiusSearch (const PointCloud &cloud, int index, double radius,
499  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
500  unsigned int max_nn = 0)
501  {
502  if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
503  return (0);
504  return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn));
505  }
506 
507  protected:
508 
509  /** \brief Filter cloud and initializes voxel structure.
510  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
511  */
512  void applyFilter (PointCloud &output) override;
513 
514  /** \brief Flag to determine if voxel structure is searchable. */
516 
517  /** \brief Minimum points contained with in a voxel to allow it to be usable. */
519 
520  /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
522 
523  /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
524  std::map<size_t, Leaf> leaves_;
525 
526  /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */
528 
529  /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */
531 
532  /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
534  };
535 }
536 
537 #ifdef PCL_NO_PRECOMPILE
538 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
539 #endif
Eigen::Matrix3d getEvecs() const
Get the eigen vectors of the voxel covariance.
boost::shared_ptr< const Filter< PointTarget > > ConstPtr
Definition: filter.h:90
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.
Definition: point_cloud.h:423
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.
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition: voxel_grid.h:462
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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.
Eigen::Vector4i max_b_
Definition: voxel_grid.h:471
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.
void filter(bool searchable=false)
Initializes voxel structure.
int getMinPointPerVoxel()
Get the minimum number of points required for a cell to be used.
typename pcl::traits::fieldList< PointTarget >::type FieldList
Definition: voxel_grid.h:488
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:177
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
Leaf * LeafPtr
Pointer to VoxelGridCovariance leaf structure.
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.
Eigen::VectorXf centroid
Nd voxel centroid.
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition: voxel_grid.h:459
Defines all the PCL implemented PointT point type structures.
Eigen::Matrix3d getCov() const
Get the voxel covariance.
PointCloudPtr getCentroids()
Get a pointcloud containing the voxel centroids.
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
const std::map< size_t, Leaf > & getLeaves()
Get the leaf structure map.
typename PointCloud::Ptr PointCloudPtr
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< PointCloud< PointT > > Ptr
Definition: point_cloud.h:441
void applyFilter(PointCloud &output) override
Filter cloud and initializes voxel structure.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Eigen::Vector4i divb_mul_
Definition: voxel_grid.h:471
void setCovEigValueInflationRatio(double min_covar_eigvalue_mult)
Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices...
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition: voxel_grid.h:456
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all the nearest neighbors of the query point in a given radius.
Eigen::Vector3d getEvals() const
Get the eigen values of the voxel covariance.
Eigen::Vector3d getMean() const
Get the voxel centroid.
boost::shared_ptr< Filter< PointTarget > > Ptr
Definition: filter.h:89
int nr_points
Number of points contained by voxel.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:442
Eigen::Vector3d evals_
Eigen values of voxel covariance matrix.
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: filter.h:180
Eigen::Matrix3d getInverseCov() const
Get the inverse of the voxel covariance.
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
KdTreeFLANN< PointT > kdtree_
KdTree generated using voxel_centroids_ (used for searching).
std::string filter_name_
The filter name.
Definition: filter.h:164
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
Definition: voxel_grid.h:471
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_.
Definition: voxel_grid.h:465
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.
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
Eigen::Matrix3d cov_
Voxel covariance matrix.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74