Point Cloud Library (PCL)  1.12.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 strucure 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: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", 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_ && !voxel_centroids_->empty ())
271  {
272  // Initiates kdtree of the centroids of voxels containing a sufficient number of points
273  kdtree_.setInputCloud (voxel_centroids_);
274  }
275  }
276 
277  /** \brief Initializes voxel structure.
278  * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
279  */
280  inline void
281  filter (bool searchable = false)
282  {
283  searchable_ = searchable;
286 
287  if (searchable_ && !voxel_centroids_->empty ())
288  {
289  // Initiates kdtree of the centroids of voxels containing a sufficient number of points
290  kdtree_.setInputCloud (voxel_centroids_);
291  }
292  }
293 
294  /** \brief Get the voxel containing point p.
295  * \param[in] index the index of the leaf structure node
296  * \return const pointer to leaf structure
297  */
298  inline LeafConstPtr
299  getLeaf (int index)
300  {
301  typename std::map<std::size_t, Leaf>::iterator leaf_iter = leaves_.find (index);
302  if (leaf_iter != leaves_.end ())
303  {
304  LeafConstPtr ret (&(leaf_iter->second));
305  return ret;
306  }
307  return nullptr;
308  }
309 
310  /** \brief Get the voxel containing point p.
311  * \param[in] p the point to get the leaf structure at
312  * \return const pointer to leaf structure
313  */
314  inline LeafConstPtr
316  {
317  // Generate index associated with p
318  int ijk0 = static_cast<int> (std::floor (p.x * inverse_leaf_size_[0]) - min_b_[0]);
319  int ijk1 = static_cast<int> (std::floor (p.y * inverse_leaf_size_[1]) - min_b_[1]);
320  int ijk2 = static_cast<int> (std::floor (p.z * inverse_leaf_size_[2]) - min_b_[2]);
321 
322  // Compute the centroid leaf index
323  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
324 
325  // Find leaf associated with index
326  typename std::map<std::size_t, Leaf>::iterator leaf_iter = leaves_.find (idx);
327  if (leaf_iter != leaves_.end ())
328  {
329  // If such a leaf exists return the pointer to the leaf structure
330  LeafConstPtr ret (&(leaf_iter->second));
331  return ret;
332  }
333  return nullptr;
334  }
335 
336  /** \brief Get the voxel containing point p.
337  * \param[in] p the point to get the leaf structure at
338  * \return const pointer to leaf structure
339  */
340  inline LeafConstPtr
341  getLeaf (Eigen::Vector3f &p)
342  {
343  // Generate index associated with p
344  int ijk0 = static_cast<int> (std::floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]);
345  int ijk1 = static_cast<int> (std::floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]);
346  int ijk2 = static_cast<int> (std::floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]);
347 
348  // Compute the centroid leaf index
349  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
350 
351  // Find leaf associated with index
352  typename std::map<std::size_t, Leaf>::iterator leaf_iter = leaves_.find (idx);
353  if (leaf_iter != leaves_.end ())
354  {
355  // If such a leaf exists return the pointer to the leaf structure
356  LeafConstPtr ret (&(leaf_iter->second));
357  return ret;
358  }
359  return nullptr;
360 
361  }
362 
363  /** \brief Get the voxels surrounding point p designated by \p relative_coordinates.
364  * \note Only voxels containing a sufficient number of points are used.
365  * \param[in] relative_coordinates 3xN matrix that represents relative coordinates of N neighboring voxels with respect to the center voxel
366  * \param[in] reference_point the point to get the leaf structure at
367  * \param[out] neighbors
368  * \return number of neighbors found
369  */
370  int
371  getNeighborhoodAtPoint (const Eigen::Matrix<int, 3, Eigen::Dynamic>& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
372 
373  /** \brief Get the voxels surrounding point p, not including the voxel containing point p.
374  * \note Only voxels containing a sufficient number of points are used.
375  * \param[in] reference_point the point to get the leaf structure at
376  * \param[out] neighbors
377  * \return number of neighbors found (up to 26)
378  */
379  int
380  getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
381 
382  /** \brief Get the voxel at p.
383  * \note Only voxels containing a sufficient number of points are used.
384  * \param[in] reference_point the point to get the leaf structure at
385  * \param[out] neighbors
386  * \return number of neighbors found (up to 1)
387  */
388  int
389  getVoxelAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
390 
391  /** \brief Get the voxel at p and its facing voxels (up to 7 voxels).
392  * \note Only voxels containing a sufficient number of points are used.
393  * \param[in] reference_point the point to get the leaf structure at
394  * \param[out] neighbors
395  * \return number of neighbors found (up to 7)
396  */
397  int
398  getFaceNeighborsAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
399 
400  /** \brief Get all 3x3x3 neighbor voxels of p (up to 27 voxels).
401  * \note Only voxels containing a sufficient number of points are used.
402  * \param[in] reference_point the point to get the leaf structure at
403  * \param[out] neighbors
404  * \return number of neighbors found (up to 27)
405  */
406  int
407  getAllNeighborsAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
408 
409  /** \brief Get the leaf structure map
410  * \return a map contataining all leaves
411  */
412  inline const std::map<std::size_t, Leaf>&
414  {
415  return leaves_;
416  }
417 
418  /** \brief Get a pointcloud containing the voxel centroids
419  * \note Only voxels containing a sufficient number of points are used.
420  * \return a map contataining all leaves
421  */
422  inline PointCloudPtr
424  {
425  return voxel_centroids_;
426  }
427 
428 
429  /** \brief Get a cloud to visualize each voxels normal distribution.
430  * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
431  */
432  void
434 
435  /** \brief Search for the k-nearest occupied voxels for the given query point.
436  * \note Only voxels containing a sufficient number of points are used.
437  * \param[in] point the given query point
438  * \param[in] k the number of neighbors to search for
439  * \param[out] k_leaves the resultant leaves of the neighboring points
440  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
441  * \return number of neighbors found
442  */
443  int
444  nearestKSearch (const PointT &point, int k,
445  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
446  {
447  k_leaves.clear ();
448 
449  // Check if kdtree has been built
450  if (!searchable_)
451  {
452  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
453  return 0;
454  }
455 
456  // Find k-nearest neighbors in the occupied voxel centroid cloud
457  Indices k_indices;
458  k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
459 
460  // Find leaves corresponding to neighbors
461  k_leaves.reserve (k);
462  for (const auto &k_index : k_indices)
463  {
464  auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
465  if (voxel == leaves_.end()) {
466  continue;
467  }
468 
469  k_leaves.push_back(&voxel->second);
470  }
471  return k_leaves.size();
472  }
473 
474  /** \brief Search for the k-nearest occupied voxels for the given query point.
475  * \note Only voxels containing a sufficient number of points are used.
476  * \param[in] cloud the given query point
477  * \param[in] index the index
478  * \param[in] k the number of neighbors to search for
479  * \param[out] k_leaves the resultant leaves of the neighboring points
480  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
481  * \return number of neighbors found
482  */
483  inline int
484  nearestKSearch (const PointCloud &cloud, int index, int k,
485  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
486  {
487  if (index >= static_cast<int> (cloud.size ()) || index < 0)
488  return (0);
489  return (nearestKSearch (cloud[index], k, k_leaves, k_sqr_distances));
490  }
491 
492 
493  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
494  * \note Only voxels containing a sufficient number of points are used.
495  * \param[in] point the given query point
496  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
497  * \param[out] k_leaves the resultant leaves of the neighboring points
498  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
499  * \param[in] max_nn
500  * \return number of neighbors found
501  */
502  int
503  radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
504  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
505  {
506  k_leaves.clear ();
507 
508  // Check if kdtree has been built
509  if (!searchable_)
510  {
511  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
512  return 0;
513  }
514 
515  // Find neighbors within radius in the occupied voxel centroid cloud
516  Indices k_indices;
517  const int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
518 
519  // Find leaves corresponding to neighbors
520  k_leaves.reserve (k);
521  for (const auto &k_index : k_indices)
522  {
523  const auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
524  if(voxel == leaves_.end()) {
525  continue;
526  }
527 
528  k_leaves.push_back(&voxel->second);
529  }
530  return k_leaves.size();
531  }
532 
533  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
534  * \note Only voxels containing a sufficient number of points are used.
535  * \param[in] cloud the given query point
536  * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
537  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
538  * \param[out] k_leaves the resultant leaves of the neighboring points
539  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
540  * \param[in] max_nn
541  * \return number of neighbors found
542  */
543  inline int
544  radiusSearch (const PointCloud &cloud, int index, double radius,
545  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
546  unsigned int max_nn = 0) const
547  {
548  if (index >= static_cast<int> (cloud.size ()) || index < 0)
549  return (0);
550  return (radiusSearch (cloud[index], radius, k_leaves, k_sqr_distances, max_nn));
551  }
552 
553  protected:
554 
555  /** \brief Filter cloud and initializes voxel structure.
556  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
557  */
558  void applyFilter (PointCloud &output) override;
559 
560  /** \brief Flag to determine if voxel structure is searchable. */
562 
563  /** \brief Minimum points contained with in a voxel to allow it to be usable. */
565 
566  /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
568 
569  /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
570  std::map<std::size_t, Leaf> leaves_;
571 
572  /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */
574 
575  /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */
577 
578  /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
580  };
581 }
582 
583 #ifdef PCL_NO_PRECOMPILE
584 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
585 #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 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: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.