Point Cloud Library (PCL)  1.14.1
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/voxel_grid.h>
41 #include <map>
42 #include <pcl/point_types.h>
43 #include <pcl/kdtree/kdtree_flann.h>
44 
45 namespace pcl
46 {
47  /** \brief A searchable voxel structure containing the mean and covariance of the data.
48  * \note For more information please see
49  * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
50  * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
51  * PhD thesis, Orebro University. Orebro Studies in Technology 36</b>
52  * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
53  */
54  template<typename PointT>
55  class VoxelGridCovariance : public VoxelGrid<PointT>
56  {
57  protected:
66 
76 
77 
78  using FieldList = typename pcl::traits::fieldList<PointT>::type;
80  using PointCloudPtr = typename PointCloud::Ptr;
82 
83  public:
84 
85  using Ptr = shared_ptr<VoxelGrid<PointT> >;
86  using ConstPtr = shared_ptr<const VoxelGrid<PointT> >;
87 
88 
89  /** \brief Simple structure to hold a centroid, covarince and the number of points in a leaf.
90  * Inverse covariance, eigen vectors and engen values are precomputed. */
91  struct Leaf
92  {
93  /** \brief Constructor.
94  * Sets \ref nr_points, \ref cov_, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref evecs_ to the identity matrix
95  */
96  Leaf () :
97  mean_ (Eigen::Vector3d::Zero ()),
98  cov_ (Eigen::Matrix3d::Zero ()),
99  icov_ (Eigen::Matrix3d::Zero ()),
100  evecs_ (Eigen::Matrix3d::Identity ()),
101  evals_ (Eigen::Vector3d::Zero ())
102  {
103  }
104 
105  /** \brief Get the voxel covariance.
106  * \return covariance matrix
107  */
108  Eigen::Matrix3d
109  getCov () const
110  {
111  return (cov_);
112  }
113 
114  /** \brief Get the inverse of the voxel covariance.
115  * \return inverse covariance matrix
116  */
117  Eigen::Matrix3d
118  getInverseCov () const
119  {
120  return (icov_);
121  }
122 
123  /** \brief Get the voxel centroid.
124  * \return centroid
125  */
126  Eigen::Vector3d
127  getMean () const
128  {
129  return (mean_);
130  }
131 
132  /** \brief Get the eigen vectors of the voxel covariance.
133  * \note Order corresponds with \ref getEvals
134  * \return matrix whose columns contain eigen vectors
135  */
136  Eigen::Matrix3d
137  getEvecs () const
138  {
139  return (evecs_);
140  }
141 
142  /** \brief Get the eigen values of the voxel covariance.
143  * \note Order corresponds with \ref getEvecs
144  * \return vector of eigen values
145  */
146  Eigen::Vector3d
147  getEvals () const
148  {
149  return (evals_);
150  }
151 
152  /** \brief Get the number of points contained by this voxel.
153  * \return number of points
154  */
155  int
156  getPointCount () const
157  {
158  return (nr_points);
159  }
160 
161  /** \brief Number of points contained by voxel */
162  int nr_points{0};
163 
164  /** \brief 3D voxel centroid */
165  Eigen::Vector3d mean_;
166 
167  /** \brief Nd voxel centroid
168  * \note Differs from \ref mean_ when color data is used
169  */
170  Eigen::VectorXf centroid;
171 
172  /** \brief Voxel covariance matrix */
173  Eigen::Matrix3d cov_;
174 
175  /** \brief Inverse of voxel covariance matrix */
176  Eigen::Matrix3d icov_;
177 
178  /** \brief Eigen vectors of voxel covariance matrix */
179  Eigen::Matrix3d evecs_;
180 
181  /** \brief Eigen values of voxel covariance matrix */
182  Eigen::Vector3d evals_;
183 
184  };
185 
186  /** \brief Pointer to VoxelGridCovariance leaf structure */
187  using LeafPtr = Leaf *;
188 
189  /** \brief Const pointer to VoxelGridCovariance leaf structure */
190  using LeafConstPtr = const Leaf *;
191 
192  public:
193 
194  /** \brief Constructor.
195  * Sets \ref leaf_size_ to 0 and \ref searchable_ to false.
196  */
198  leaves_ (),
199  voxel_centroids_ (),
200  kdtree_ ()
201  {
202  downsample_all_data_ = false;
203  save_leaf_layout_ = false;
204  leaf_size_.setZero ();
205  min_b_.setZero ();
206  max_b_.setZero ();
207  filter_name_ = "VoxelGridCovariance";
208  }
209 
210  /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation).
211  * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
212  */
213  inline void
214  setMinPointPerVoxel (int min_points_per_voxel)
215  {
216  if(min_points_per_voxel > 2)
217  {
218  min_points_per_voxel_ = min_points_per_voxel;
219  }
220  else
221  {
222  PCL_WARN ("[%s::setMinPointPerVoxel] Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3\n", this->getClassName ().c_str ());
224  }
225  }
226 
227  /** \brief Get the minimum number of points required for a cell to be used.
228  * \return the minimum number of points for required for a voxel to be used
229  */
230  inline int
232  {
233  return min_points_per_voxel_;
234  }
235 
236  /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
237  * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues
238  */
239  inline void
240  setCovEigValueInflationRatio (double min_covar_eigvalue_mult)
241  {
242  min_covar_eigvalue_mult_ = min_covar_eigvalue_mult;
243  }
244 
245  /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
246  * \return the minimum allowable ratio between eigenvalues
247  */
248  inline double
250  {
252  }
253 
254  /** \brief Filter cloud and initializes voxel structure.
255  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
256  * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
257  */
258  inline void
259  filter (PointCloud &output, bool searchable = false)
260  {
261  searchable_ = searchable;
262  applyFilter (output);
263 
264  voxel_centroids_ = PointCloudPtr (new PointCloud (output));
265 
266  if (searchable_)
267  {
268  if (voxel_centroids_->empty ()) {
269  PCL_WARN ("[%s::filter] No voxels with a sufficient number of points. Grid will not be searchable. You can try reducing the min number of points required per voxel or increasing the voxel/leaf size.\n", this->getClassName ().c_str ());
270  searchable_ = false;
271  } else {
272  // Initiates kdtree of the centroids of voxels containing a sufficient number of points
274  }
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_)
289  {
290  if (voxel_centroids_->empty ()) {
291  PCL_WARN ("[%s::filter] No voxels with a sufficient number of points. Grid will not be searchable. You can try reducing the min number of points required per voxel or increasing the voxel/leaf size\n", this->getClassName ().c_str ());
292  searchable_ = false;
293  } else {
294  // Initiates kdtree of the centroids of voxels containing a sufficient number of points
296  }
297  }
298  }
299 
300  /** \brief Get the voxel containing point p.
301  * \param[in] index the index of the leaf structure node
302  * \return const pointer to leaf structure
303  */
304  inline LeafConstPtr
305  getLeaf (int index)
306  {
307  auto leaf_iter = leaves_.find (index);
308  if (leaf_iter != leaves_.end ())
309  {
310  LeafConstPtr ret (&(leaf_iter->second));
311  return ret;
312  }
313  return nullptr;
314  }
315 
316  /** \brief Get the voxel containing point p.
317  * \param[in] p the point to get the leaf structure at
318  * \return const pointer to leaf structure
319  */
320  inline LeafConstPtr
322  {
323  // Generate index associated with p
324  int ijk0 = static_cast<int> (std::floor (p.x * inverse_leaf_size_[0]) - min_b_[0]);
325  int ijk1 = static_cast<int> (std::floor (p.y * inverse_leaf_size_[1]) - min_b_[1]);
326  int ijk2 = static_cast<int> (std::floor (p.z * inverse_leaf_size_[2]) - min_b_[2]);
327 
328  // Compute the centroid leaf index
329  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
330 
331  // Find leaf associated with index
332  auto leaf_iter = leaves_.find (idx);
333  if (leaf_iter != leaves_.end ())
334  {
335  // If such a leaf exists return the pointer to the leaf structure
336  LeafConstPtr ret (&(leaf_iter->second));
337  return ret;
338  }
339  return nullptr;
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> (std::floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]);
351  int ijk1 = static_cast<int> (std::floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]);
352  int ijk2 = static_cast<int> (std::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  auto 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  return nullptr;
366 
367  }
368 
369  /** \brief Get the voxels surrounding point p designated by \p relative_coordinates.
370  * \note Only voxels containing a sufficient number of points are used.
371  * \param[in] relative_coordinates 3xN matrix that represents relative coordinates of N neighboring voxels with respect to the center voxel
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 Eigen::Matrix<int, 3, Eigen::Dynamic>& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
378 
379  /** \brief Get the voxels surrounding point p, not including the voxel containing point p.
380  * \note Only voxels containing a sufficient number of points are used.
381  * \param[in] reference_point the point to get the leaf structure at
382  * \param[out] neighbors
383  * \return number of neighbors found (up to 26)
384  */
385  int
386  getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
387 
388  /** \brief Get the voxel at p.
389  * \note Only voxels containing a sufficient number of points are used.
390  * \param[in] reference_point the point to get the leaf structure at
391  * \param[out] neighbors
392  * \return number of neighbors found (up to 1)
393  */
394  int
395  getVoxelAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
396 
397  /** \brief Get the voxel at p and its facing voxels (up to 7 voxels).
398  * \note Only voxels containing a sufficient number of points are used.
399  * \param[in] reference_point the point to get the leaf structure at
400  * \param[out] neighbors
401  * \return number of neighbors found (up to 7)
402  */
403  int
404  getFaceNeighborsAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
405 
406  /** \brief Get all 3x3x3 neighbor voxels of p (up to 27 voxels).
407  * \note Only voxels containing a sufficient number of points are used.
408  * \param[in] reference_point the point to get the leaf structure at
409  * \param[out] neighbors
410  * \return number of neighbors found (up to 27)
411  */
412  int
413  getAllNeighborsAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
414 
415  /** \brief Get the leaf structure map
416  * \return a map contataining all leaves
417  */
418  inline const std::map<std::size_t, Leaf>&
420  {
421  return leaves_;
422  }
423 
424  /** \brief Get a pointcloud containing the voxel centroids
425  * \note Only voxels containing a sufficient number of points are used.
426  * \return a map contataining all leaves
427  */
428  inline PointCloudPtr
430  {
431  return voxel_centroids_;
432  }
433 
434 
435  /** \brief Get a cloud to visualize each voxels normal distribution.
436  * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
437  */
438  void
440 
441  /** \brief Search for the k-nearest occupied voxels for the given query point.
442  * \note Only voxels containing a sufficient number of points are used.
443  * \param[in] point the given query point
444  * \param[in] k the number of neighbors to search for
445  * \param[out] k_leaves the resultant leaves of the neighboring points
446  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
447  * \return number of neighbors found
448  */
449  int
450  nearestKSearch (const PointT &point, int k,
451  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
452  {
453  k_leaves.clear ();
454 
455  // Check if kdtree has been built
456  if (!searchable_)
457  {
458  PCL_WARN ("[%s::nearestKSearch] Not Searchable\n", this->getClassName ().c_str ());
459  return 0;
460  }
461 
462  // Find k-nearest neighbors in the occupied voxel centroid cloud
463  Indices k_indices (k);
464  k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
465 
466  // Find leaves corresponding to neighbors
467  k_leaves.reserve (k);
468  for (const auto &k_index : k_indices)
469  {
470  auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
471  if (voxel == leaves_.end()) {
472  continue;
473  }
474 
475  k_leaves.push_back(&voxel->second);
476  }
477  return k_leaves.size();
478  }
479 
480  /** \brief Search for the k-nearest occupied voxels for the given query point.
481  * \note Only voxels containing a sufficient number of points are used.
482  * \param[in] cloud the given query point
483  * \param[in] index the index
484  * \param[in] k the number of neighbors to search for
485  * \param[out] k_leaves the resultant leaves of the neighboring points
486  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
487  * \return number of neighbors found
488  */
489  inline int
490  nearestKSearch (const PointCloud &cloud, int index, int k,
491  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
492  {
493  if (index >= static_cast<int> (cloud.size ()) || index < 0)
494  return (0);
495  return (nearestKSearch (cloud[index], k, k_leaves, k_sqr_distances));
496  }
497 
498 
499  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
500  * \note Only voxels containing a sufficient number of points are used.
501  * \param[in] point the given query point
502  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
503  * \param[out] k_leaves the resultant leaves of the neighboring points
504  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
505  * \param[in] max_nn
506  * \return number of neighbors found
507  */
508  int
509  radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
510  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
511  {
512  k_leaves.clear ();
513 
514  // Check if kdtree has been built
515  if (!searchable_)
516  {
517  PCL_WARN ("[%s::radiusSearch] Not Searchable\n", this->getClassName ().c_str ());
518  return 0;
519  }
520 
521  // Find neighbors within radius in the occupied voxel centroid cloud
522  Indices k_indices;
523  const int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
524 
525  // Find leaves corresponding to neighbors
526  k_leaves.reserve (k);
527  for (const auto &k_index : k_indices)
528  {
529  const auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
530  if(voxel == leaves_.end()) {
531  continue;
532  }
533 
534  k_leaves.push_back(&voxel->second);
535  }
536  return k_leaves.size();
537  }
538 
539  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
540  * \note Only voxels containing a sufficient number of points are used.
541  * \param[in] cloud the given query point
542  * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
543  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
544  * \param[out] k_leaves the resultant leaves of the neighboring points
545  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
546  * \param[in] max_nn
547  * \return number of neighbors found
548  */
549  inline int
550  radiusSearch (const PointCloud &cloud, int index, double radius,
551  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
552  unsigned int max_nn = 0) const
553  {
554  if (index >= static_cast<int> (cloud.size ()) || index < 0)
555  return (0);
556  return (radiusSearch (cloud[index], radius, k_leaves, k_sqr_distances, max_nn));
557  }
558 
559  protected:
560 
561  /** \brief Filter cloud and initializes voxel structure.
562  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
563  */
564  void applyFilter (PointCloud &output) override;
565 
566  /** \brief Flag to determine if voxel structure is searchable. */
567  bool searchable_{true};
568 
569  /** \brief Minimum points contained with in a voxel to allow it to be usable. */
571 
572  /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
574 
575  /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
576  std::map<std::size_t, Leaf> leaves_;
577 
578  /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */
580 
581  /** \brief Indices of leaf structures associated with each point in \ref voxel_centroids_ (used for searching). */
583 
584  /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
586  };
587 }
588 
589 #ifdef PCL_NO_PRECOMPILE
590 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
591 #endif
Eigen::Vector3d getMean() const
Get the voxel centroid.
Eigen::Vector3d getEvals() const
Get the eigen values of the voxel covariance.
LeafConstPtr getLeaf(Eigen::Vector3f &p)
Get the voxel containing point p.
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
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.
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition: voxel_grid.h:489
Eigen::Vector3d mean_
3D voxel centroid
Eigen::Matrix3d getEvecs() const
Get the eigen vectors of the voxel covariance.
Eigen::Matrix3d icov_
Inverse of voxel covariance matrix.
int radiusSearch(const PointT &point, double radius, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest occupied voxels of the query point in a given radius.
Eigen::Vector4i max_b_
Definition: voxel_grid.h:498
double min_covar_eigvalue_mult_
Minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
Eigen::Matrix3d getCov() const
Get the voxel covariance.
const std::map< std::size_t, Leaf > & getLeaves()
Get the leaf structure map.
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:515
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:209
Definition: bfgs.h:10
Leaf * LeafPtr
Pointer to VoxelGridCovariance leaf structure.
std::vector< int > voxel_centroids_leaf_indices_
Indices of leaf structures 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.
int getNeighborhoodAtPoint(const Eigen::Matrix< int, 3, Eigen::Dynamic > &relative_coordinates, const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxels surrounding point p designated by relative_coordinates.
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:486
shared_ptr< Filter< PointTarget > > Ptr
Definition: filter.h:83
int getFaceNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p and its facing voxels (up to 7 voxels).
PointCloudPtr getCentroids()
Get a pointcloud containing the voxel centroids.
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
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.
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.
int nearestKSearch(const PointCloud &cloud, int index, int k, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances) const
Search for the k-nearest occupied voxels for the given query point.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Eigen::Vector4i divb_mul_
Definition: voxel_grid.h:498
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:483
int nr_points
Number of points contained by voxel.
std::map< std::size_t, Leaf > leaves_
Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of poin...
Eigen::Vector3d evals_
Eigen values of voxel covariance matrix.
int getVoxelAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p.
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
KdTreeFLANN< PointT > kdtree_
KdTree generated using voxel_centroids_ (used for searching).
std::string filter_name_
The filter name.
Definition: filter.h:158
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
Definition: voxel_grid.h:498
shared_ptr< const Filter< PointTarget > > ConstPtr
Definition: filter.h:84
A point structure representing Euclidean xyz coordinates, and the RGB color.
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 ...
std::size_t size() const
Definition: point_cloud.h:443
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
Definition: voxel_grid.h:492
A searchable voxel structure containing the mean and covariance of the data.
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
Eigen::Matrix3d cov_
Voxel covariance matrix.
int getAllNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get all 3x3x3 neighbor voxels of p (up to 27 voxels).
int radiusSearch(const PointT &point, double radius, Indices &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.
int nearestKSearch(const PointT &point, int k, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances) const
Search for the k-nearest occupied voxels for the given query point.
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) const
Search for all the nearest occupied voxels of the query point in a given radius.
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: filter.h:174
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
int nearestKSearch(const PointT &point, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
int getPointCount() const
Get the number of points contained by this voxel.