Point Cloud Library (PCL)  1.14.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/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
273  kdtree_.setInputCloud (voxel_centroids_);
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
295  kdtree_.setInputCloud (voxel_centroids_);
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  * \param[in] pnt_per_cell how many points should be generated for each cell
438  */
439  void
440  getDisplayCloud (pcl::PointCloud<PointXYZ>& cell_cloud, int pnt_per_cell = 1000) const;
441 
442  /** \brief Search for the k-nearest occupied voxels for the given query point.
443  * \note Only voxels containing a sufficient number of points are used.
444  * \param[in] point the given query point
445  * \param[in] k the number of neighbors to search for
446  * \param[out] k_leaves the resultant leaves of the neighboring points
447  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
448  * \return number of neighbors found
449  */
450  int
451  nearestKSearch (const PointT &point, int k,
452  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
453  {
454  k_leaves.clear ();
455 
456  // Check if kdtree has been built
457  if (!searchable_)
458  {
459  PCL_WARN ("[%s::nearestKSearch] Not Searchable\n", this->getClassName ().c_str ());
460  return 0;
461  }
462 
463  // Find k-nearest neighbors in the occupied voxel centroid cloud
464  Indices k_indices (k);
465  k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
466 
467  // Find leaves corresponding to neighbors
468  k_leaves.reserve (k);
469  for (const auto &k_index : k_indices)
470  {
471  auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
472  if (voxel == leaves_.end()) {
473  continue;
474  }
475 
476  k_leaves.push_back(&voxel->second);
477  }
478  return k_leaves.size();
479  }
480 
481  /** \brief Search for the k-nearest occupied voxels for the given query point.
482  * \note Only voxels containing a sufficient number of points are used.
483  * \param[in] cloud the given query point
484  * \param[in] index the index
485  * \param[in] k the number of neighbors to search for
486  * \param[out] k_leaves the resultant leaves of the neighboring points
487  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
488  * \return number of neighbors found
489  */
490  inline int
491  nearestKSearch (const PointCloud &cloud, int index, int k,
492  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
493  {
494  if (index >= static_cast<int> (cloud.size ()) || index < 0)
495  return (0);
496  return (nearestKSearch (cloud[index], k, k_leaves, k_sqr_distances));
497  }
498 
499 
500  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
501  * \note Only voxels containing a sufficient number of points are used.
502  * \param[in] point the given query point
503  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
504  * \param[out] k_leaves the resultant leaves of the neighboring points
505  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
506  * \param[in] max_nn
507  * \return number of neighbors found
508  */
509  int
510  radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
511  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
512  {
513  k_leaves.clear ();
514 
515  // Check if kdtree has been built
516  if (!searchable_)
517  {
518  PCL_WARN ("[%s::radiusSearch] Not Searchable\n", this->getClassName ().c_str ());
519  return 0;
520  }
521 
522  // Find neighbors within radius in the occupied voxel centroid cloud
523  Indices k_indices;
524  const int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
525 
526  // Find leaves corresponding to neighbors
527  k_leaves.reserve (k);
528  for (const auto &k_index : k_indices)
529  {
530  const auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
531  if(voxel == leaves_.end()) {
532  continue;
533  }
534 
535  k_leaves.push_back(&voxel->second);
536  }
537  return k_leaves.size();
538  }
539 
540  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
541  * \note Only voxels containing a sufficient number of points are used.
542  * \param[in] cloud the given query point
543  * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
544  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
545  * \param[out] k_leaves the resultant leaves of the neighboring points
546  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
547  * \param[in] max_nn
548  * \return number of neighbors found
549  */
550  inline int
551  radiusSearch (const PointCloud &cloud, int index, double radius,
552  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
553  unsigned int max_nn = 0) const
554  {
555  if (index >= static_cast<int> (cloud.size ()) || index < 0)
556  return (0);
557  return (radiusSearch (cloud[index], radius, k_leaves, k_sqr_distances, max_nn));
558  }
559 
560  protected:
561 
562  /** \brief Filter cloud and initializes voxel structure.
563  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
564  */
565  void applyFilter (PointCloud &output) override;
566 
567  /** \brief Flag to determine if voxel structure is searchable. */
568  bool searchable_{true};
569 
570  /** \brief Minimum points contained with in a voxel to allow it to be usable. */
572 
573  /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
575 
576  /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
577  std::map<std::size_t, Leaf> leaves_;
578 
579  /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */
581 
582  /** \brief Indices of leaf structures associated with each point in \ref voxel_centroids_ (used for searching). */
584 
585  /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
587  };
588 }
589 
590 #ifdef PCL_NO_PRECOMPILE
591 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
592 #endif
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: filter.h:174
shared_ptr< Filter< PointT > > Ptr
Definition: filter.h:83
shared_ptr< const Filter< PointT > > ConstPtr
Definition: filter.h:84
std::string filter_name_
The filter name.
Definition: filter.h:158
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
std::size_t size() const
Definition: point_cloud.h:443
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
A searchable voxel structure containing the mean and covariance of the data.
void applyFilter(PointCloud &output) override
Filter cloud and initializes voxel structure.
double getCovEigValueInflationRatio()
Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
LeafConstPtr getLeaf(int index)
Get the voxel containing point p.
int min_points_per_voxel_
Minimum points contained with in a voxel to allow it to be usable.
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.
PointCloudPtr voxel_centroids_
Point cloud containing centroids of voxels containing atleast minimum number of points.
typename PointCloud::Ptr PointCloudPtr
void getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud, int pnt_per_cell=1000) const
Get a cloud to visualize each voxels normal distribution.
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 ...
int getAllNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get all 3x3x3 neighbor voxels of p (up to 27 voxels).
LeafConstPtr getLeaf(Eigen::Vector3f &p)
Get the voxel containing point p.
void filter(PointCloud &output, bool searchable=false)
Filter cloud and initializes voxel structure.
int getVoxelAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p.
bool searchable_
Flag to determine if voxel structure is searchable.
double min_covar_eigvalue_mult_
Minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
KdTreeFLANN< PointT > kdtree_
KdTree generated using voxel_centroids_ (used for searching).
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 Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
void filter(bool searchable=false)
Initializes voxel structure.
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.
void setCovEigValueInflationRatio(double min_covar_eigvalue_mult)
Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
int getMinPointPerVoxel()
Get the minimum number of points required for a cell to be used.
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.
std::vector< int > voxel_centroids_leaf_indices_
Indices of leaf structures associated with each point in voxel_centroids_ (used for searching).
PointCloudPtr getCentroids()
Get a pointcloud containing the voxel centroids.
std::map< std::size_t, Leaf > leaves_
Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of poin...
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.
const std::map< std::size_t, Leaf > & getLeaves()
Get the leaf structure map.
int getFaceNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p and its facing voxels (up to 7 voxels).
LeafConstPtr getLeaf(PointT &p)
Get the voxel containing point p.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
Definition: voxel_grid.h:210
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::Vector4i max_b_
Definition: voxel_grid.h:498
Eigen::Vector4i divb_mul_
Definition: voxel_grid.h:498
typename pcl::traits::fieldList< PointT >::type FieldList
Definition: voxel_grid.h:515
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
Definition: voxel_grid.h:492
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier.
Definition: voxel_grid.h:498
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition: voxel_grid.h:483
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition: voxel_grid.h:486
Defines all the PCL implemented PointT point type structures.
Definition: bfgs.h:10
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.
Simple structure to hold a centroid, covarince and the number of points in a leaf.
Eigen::Matrix3d getEvecs() const
Get the eigen vectors of the voxel covariance.
int nr_points
Number of points contained by voxel.
Eigen::Vector3d mean_
3D voxel centroid
Eigen::Matrix3d icov_
Inverse of voxel covariance matrix.
Eigen::Matrix3d getCov() const
Get the voxel covariance.
Eigen::Vector3d getEvals() const
Get the eigen values of the voxel covariance.
Eigen::VectorXf centroid
Nd voxel centroid.
Eigen::Matrix3d getInverseCov() const
Get the inverse of the voxel covariance.
Eigen::Vector3d evals_
Eigen values of voxel covariance matrix.
Eigen::Vector3d getMean() const
Get the voxel centroid.
int getPointCount() const
Get the number of points contained by this voxel.
Eigen::Matrix3d cov_
Voxel covariance matrix.
Eigen::Matrix3d evecs_
Eigen vectors of voxel covariance matrix.