Point Cloud Library (PCL)  1.11.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/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 
79  using FieldList = typename pcl::traits::fieldList<PointT>::type;
81  using PointCloudPtr = typename PointCloud::Ptr;
83 
84  public:
85 
86  using Ptr = shared_ptr<VoxelGrid<PointT> >;
87  using ConstPtr = 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
274  kdtree_.setInputCloud (voxel_centroids_);
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
291  kdtree_.setInputCloud (voxel_centroids_);
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<std::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<std::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<std::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 designated by #relative_coordinates.
365  * \note Only voxels containing a sufficient number of points are used.
366  * \param[in] relative_coordinates 3xN matrix that represents relative coordinates of N neighboring voxels with respect to the center voxel
367  * \param[in] reference_point the point to get the leaf structure at
368  * \param[out] neighbors
369  * \return number of neighbors found
370  */
371  int
372  getNeighborhoodAtPoint (const Eigen::Matrix<int, 3, Eigen::Dynamic>& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
373 
374  /** \brief Get the voxels surrounding point p, not including the voxel containing point p.
375  * \note Only voxels containing a sufficient number of points are used.
376  * \param[in] reference_point the point to get the leaf structure at
377  * \param[out] neighbors
378  * \return number of neighbors found (up to 26)
379  */
380  int
381  getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
382 
383  /** \brief Get the voxel at 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 1)
388  */
389  int
390  getVoxelAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
391 
392  /** \brief Get the voxel at p and its facing voxels (up to 7 voxels).
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 7)
397  */
398  int
399  getFaceNeighborsAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
400 
401  /** \brief Get all 3x3x3 neighbor voxels of p (up to 27 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 27)
406  */
407  int
408  getAllNeighborsAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
409 
410  /** \brief Get the leaf structure map
411  * \return a map contataining all leaves
412  */
413  inline const std::map<std::size_t, Leaf>&
415  {
416  return leaves_;
417  }
418 
419  /** \brief Get a pointcloud containing the voxel centroids
420  * \note Only voxels containing a sufficient number of points are used.
421  * \return a map contataining all leaves
422  */
423  inline PointCloudPtr
425  {
426  return voxel_centroids_;
427  }
428 
429 
430  /** \brief Get a cloud to visualize each voxels normal distribution.
431  * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
432  */
433  void
435 
436  /** \brief Search for the k-nearest occupied voxels for the given query point.
437  * \note Only voxels containing a sufficient number of points are used.
438  * \param[in] point the given query point
439  * \param[in] k the number of neighbors to search for
440  * \param[out] k_leaves the resultant leaves of the neighboring points
441  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
442  * \return number of neighbors found
443  */
444  int
445  nearestKSearch (const PointT &point, int k,
446  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
447  {
448  k_leaves.clear ();
449 
450  // Check if kdtree has been built
451  if (!searchable_)
452  {
453  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
454  return 0;
455  }
456 
457  // Find k-nearest neighbors in the occupied voxel centroid cloud
458  std::vector<int> k_indices;
459  k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
460 
461  // Find leaves corresponding to neighbors
462  k_leaves.reserve (k);
463  for (const int &k_index : k_indices)
464  {
465  auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
466  if (voxel == leaves_.end()) {
467  continue;
468  }
469 
470  k_leaves.push_back(&voxel->second);
471  }
472  return k_leaves.size();
473  }
474 
475  /** \brief Search for the k-nearest occupied voxels for the given query point.
476  * \note Only voxels containing a sufficient number of points are used.
477  * \param[in] cloud the given query point
478  * \param[in] index the index
479  * \param[in] k the number of neighbors to search for
480  * \param[out] k_leaves the resultant leaves of the neighboring points
481  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
482  * \return number of neighbors found
483  */
484  inline int
485  nearestKSearch (const PointCloud &cloud, int index, int k,
486  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
487  {
488  if (index >= static_cast<int> (cloud.size ()) || index < 0)
489  return (0);
490  return (nearestKSearch (cloud[index], k, k_leaves, k_sqr_distances));
491  }
492 
493 
494  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
495  * \note Only voxels containing a sufficient number of points are used.
496  * \param[in] point the given query point
497  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
498  * \param[out] k_leaves the resultant leaves of the neighboring points
499  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
500  * \param[in] max_nn
501  * \return number of neighbors found
502  */
503  int
504  radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
505  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
506  {
507  k_leaves.clear ();
508 
509  // Check if kdtree has been built
510  if (!searchable_)
511  {
512  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
513  return 0;
514  }
515 
516  // Find neighbors within radius in the occupied voxel centroid cloud
517  std::vector<int> k_indices;
518  const int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
519 
520  // Find leaves corresponding to neighbors
521  k_leaves.reserve (k);
522  for (const int &k_index : k_indices)
523  {
524  const auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
525  if(voxel == leaves_.end()) {
526  continue;
527  }
528 
529  k_leaves.push_back(&voxel->second);
530  }
531  return k_leaves.size();
532  }
533 
534  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
535  * \note Only voxels containing a sufficient number of points are used.
536  * \param[in] cloud the given query point
537  * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
538  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
539  * \param[out] k_leaves the resultant leaves of the neighboring points
540  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
541  * \param[in] max_nn
542  * \return number of neighbors found
543  */
544  inline int
545  radiusSearch (const PointCloud &cloud, int index, double radius,
546  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
547  unsigned int max_nn = 0) const
548  {
549  if (index >= static_cast<int> (cloud.size ()) || index < 0)
550  return (0);
551  return (radiusSearch (cloud[index], radius, k_leaves, k_sqr_distances, max_nn));
552  }
553 
554  protected:
555 
556  /** \brief Filter cloud and initializes voxel structure.
557  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
558  */
559  void applyFilter (PointCloud &output) override;
560 
561  /** \brief Flag to determine if voxel structure is searchable. */
563 
564  /** \brief Minimum points contained with in a voxel to allow it to be usable. */
566 
567  /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
569 
570  /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
571  std::map<std::size_t, Leaf> leaves_;
572 
573  /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */
575 
576  /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */
578 
579  /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
581  };
582 }
583 
584 #ifdef PCL_NO_PRECOMPILE
585 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
586 #endif
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: filter.h:177
shared_ptr< Filter< PointT > > Ptr
Definition: filter.h:86
shared_ptr< const Filter< PointT > > ConstPtr
Definition: filter.h:87
std::string filter_name_
The filter name.
Definition: filter.h:161
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:68
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:76
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:77
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:181
std::size_t size() const
Definition: point_cloud.h:459
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
A searchable voxel strucure 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 structurs 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:178
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition: voxel_grid.h:462
Eigen::Vector4i max_b_
Definition: voxel_grid.h:471
Eigen::Vector4i divb_mul_
Definition: voxel_grid.h:471
typename pcl::traits::fieldList< PointT >::type FieldList
Definition: voxel_grid.h:488
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
Definition: voxel_grid.h:465
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier.
Definition: voxel_grid.h:471
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition: voxel_grid.h:456
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.
Definition: bfgs.h:10
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.