Point Cloud Library (PCL)  1.11.0
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 
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, not including the voxel containing point p.
365  * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice).
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 PointT& reference_point, std::vector<LeafConstPtr> &neighbors);
372 
373  /** \brief Get the leaf structure map
374  * \return a map contataining all leaves
375  */
376  inline const std::map<std::size_t, Leaf>&
378  {
379  return leaves_;
380  }
381 
382  /** \brief Get a pointcloud containing the voxel centroids
383  * \note Only voxels containing a sufficient number of points are used.
384  * \return a map contataining all leaves
385  */
386  inline PointCloudPtr
388  {
389  return voxel_centroids_;
390  }
391 
392 
393  /** \brief Get a cloud to visualize each voxels normal distribution.
394  * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
395  */
396  void
398 
399  /** \brief Search for the k-nearest occupied voxels for the given query point.
400  * \note Only voxels containing a sufficient number of points are used.
401  * \param[in] point the given query point
402  * \param[in] k the number of neighbors to search for
403  * \param[out] k_leaves the resultant leaves of the neighboring points
404  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
405  * \return number of neighbors found
406  */
407  int
408  nearestKSearch (const PointT &point, int k,
409  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
410  {
411  k_leaves.clear ();
412 
413  // Check if kdtree has been built
414  if (!searchable_)
415  {
416  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
417  return 0;
418  }
419 
420  // Find k-nearest neighbors in the occupied voxel centroid cloud
421  std::vector<int> k_indices;
422  k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
423 
424  // Find leaves corresponding to neighbors
425  k_leaves.reserve (k);
426  for (const int &k_index : k_indices)
427  {
428  k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[k_index]]);
429  }
430  return k;
431  }
432 
433  /** \brief Search for the k-nearest occupied voxels for the given query point.
434  * \note Only voxels containing a sufficient number of points are used.
435  * \param[in] cloud the given query point
436  * \param[in] index the index
437  * \param[in] k the number of neighbors to search for
438  * \param[out] k_leaves the resultant leaves of the neighboring points
439  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
440  * \return number of neighbors found
441  */
442  inline int
443  nearestKSearch (const PointCloud &cloud, int index, int k,
444  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
445  {
446  if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
447  return (0);
448  return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances));
449  }
450 
451 
452  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
453  * \note Only voxels containing a sufficient number of points are used.
454  * \param[in] point the given query point
455  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
456  * \param[out] k_leaves the resultant leaves of the neighboring points
457  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
458  * \param[in] max_nn
459  * \return number of neighbors found
460  */
461  int
462  radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
463  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
464  {
465  k_leaves.clear ();
466 
467  // Check if kdtree has been built
468  if (!searchable_)
469  {
470  PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
471  return 0;
472  }
473 
474  // Find neighbors within radius in the occupied voxel centroid cloud
475  std::vector<int> k_indices;
476  int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
477 
478  // Find leaves corresponding to neighbors
479  k_leaves.reserve (k);
480  for (const int &k_index : k_indices)
481  {
482  k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[k_index]]);
483  }
484  return k;
485  }
486 
487  /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
488  * \note Only voxels containing a sufficient number of points are used.
489  * \param[in] cloud the given query point
490  * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
491  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
492  * \param[out] k_leaves the resultant leaves of the neighboring points
493  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
494  * \param[in] max_nn
495  * \return number of neighbors found
496  */
497  inline int
498  radiusSearch (const PointCloud &cloud, int index, double radius,
499  std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
500  unsigned int max_nn = 0)
501  {
502  if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
503  return (0);
504  return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn));
505  }
506 
507  protected:
508 
509  /** \brief Filter cloud and initializes voxel structure.
510  * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
511  */
512  void applyFilter (PointCloud &output) override;
513 
514  /** \brief Flag to determine if voxel structure is searchable. */
516 
517  /** \brief Minimum points contained with in a voxel to allow it to be usable. */
519 
520  /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
522 
523  /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
524  std::map<std::size_t, Leaf> leaves_;
525 
526  /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */
528 
529  /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */
531 
532  /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
534  };
535 }
536 
537 #ifdef PCL_NO_PRECOMPILE
538 #include <pcl/filters/impl/voxel_grid_covariance.hpp>
539 #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: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:180
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
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 PointCloud &cloud, int index, int k, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances)
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 radiusSearch(const PointT &point, double radius, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all the nearest occupied voxels of the query point in a given radius.
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 radiusSearch(const PointCloud &cloud, int index, double radius, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all the nearest occupied voxels of the query point in a given radius.
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).
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
int nearestKSearch(const PointT &point, int k, std::vector< LeafConstPtr > &k_leaves, std::vector< float > &k_sqr_distances)
Search for the k-nearest occupied voxels for the given query point.
void filter(bool searchable=false)
Initializes voxel structure.
int getNeighborhoodAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors)
Get the voxels surrounding point p, not including the voxel containing point p.
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.
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...
const std::map< std::size_t, Leaf > & getLeaves()
Get the leaf structure map.
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.