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