Point Cloud Library (PCL)  1.11.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
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
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 #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
pcl::VoxelGrid
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
Definition: voxel_grid.h:176
pcl
Definition: convolution.h:46
pcl::VoxelGridCovariance::radiusSearch
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.
Definition: voxel_grid_covariance.h:544
pcl::VoxelGridCovariance::VoxelGridCovariance
VoxelGridCovariance()
Constructor.
Definition: voxel_grid_covariance.h:198
point_types.h
pcl::Filter< PointTarget >::Ptr
shared_ptr< Filter< PointTarget > > Ptr
Definition: filter.h:83
Eigen
Definition: bfgs.h:10
pcl::PCLBase< PointTarget >::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
pcl::PCLBase< PointTarget >::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
pcl::VoxelGridCovariance::applyFilter
void applyFilter(PointCloud &output) override
Filter cloud and initializes voxel structure.
Definition: voxel_grid_covariance.hpp:53
pcl::VoxelGridCovariance< PointTarget >::LeafConstPtr
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
Definition: voxel_grid_covariance.h:191
pcl::VoxelGridCovariance::getCovEigValueInflationRatio
double getCovEigValueInflationRatio()
Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
Definition: voxel_grid_covariance.h:253
pcl::VoxelGridCovariance::Leaf::icov_
Eigen::Matrix3d icov_
Inverse of voxel covariance matrix.
Definition: voxel_grid_covariance.h:177
pcl::VoxelGrid::downsample_all_data_
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition: voxel_grid.h:461
pcl::VoxelGridCovariance::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: voxel_grid_covariance.h:80
pcl::PointCloud< PointTarget >
pcl::VoxelGrid::inverse_leaf_size_
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition: voxel_grid.h:458
pcl::VoxelGrid< PointTarget >::FieldList
typename pcl::traits::fieldList< PointTarget >::type FieldList
Definition: voxel_grid.h:487
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::VoxelGridCovariance::getLeaf
LeafConstPtr getLeaf(PointT &p)
Get the voxel containing point p.
Definition: voxel_grid_covariance.h:315
pcl::VoxelGridCovariance::getLeaf
LeafConstPtr getLeaf(int index)
Get the voxel containing point p.
Definition: voxel_grid_covariance.h:299
pcl::VoxelGrid::save_leaf_layout_
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
Definition: voxel_grid.h:464
pcl::KdTreeFLANN::nearestKSearch
int nearestKSearch(const PointT &point, unsigned int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
Definition: kdtree_flann.hpp:132
pcl::VoxelGridCovariance::voxel_centroids_
PointCloudPtr voxel_centroids_
Point cloud containing centroids of voxels containing atleast minimum number of points.
Definition: voxel_grid_covariance.h:573
pcl::VoxelGridCovariance::Leaf::getMean
Eigen::Vector3d getMean() const
Get the voxel centroid.
Definition: voxel_grid_covariance.h:128
pcl::VoxelGridCovariance::radiusSearch
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.
Definition: voxel_grid_covariance.h:503
pcl::VoxelGridCovariance::filter
void filter(bool searchable=false)
Initializes voxel structure.
Definition: voxel_grid_covariance.h:281
pcl::VoxelGridCovariance::voxel_centroids_leaf_indices_
std::vector< int > voxel_centroids_leaf_indices_
Indices of leaf structurs associated with each point in voxel_centroids_ (used for searching).
Definition: voxel_grid_covariance.h:576
pcl::VoxelGridCovariance::kdtree_
KdTreeFLANN< PointT > kdtree_
KdTree generated using voxel_centroids_ (used for searching).
Definition: voxel_grid_covariance.h:579
pcl::VoxelGridCovariance::Leaf::centroid
Eigen::VectorXf centroid
Nd voxel centroid.
Definition: voxel_grid_covariance.h:171
pcl::VoxelGridCovariance::getCentroids
PointCloudPtr getCentroids()
Get a pointcloud containing the voxel centroids.
Definition: voxel_grid_covariance.h:423
pcl::VoxelGridCovariance::getDisplayCloud
void getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud)
Get a cloud to visualize each voxels normal distribution.
Definition: voxel_grid_covariance.hpp:444
pcl::VoxelGridCovariance< PointTarget >::LeafPtr
Leaf * LeafPtr
Pointer to VoxelGridCovariance leaf structure.
Definition: voxel_grid_covariance.h:188
pcl::VoxelGridCovariance::getLeaves
const std::map< std::size_t, Leaf > & getLeaves()
Get the leaf structure map.
Definition: voxel_grid_covariance.h:413
pcl::VoxelGridCovariance::Leaf::getEvecs
Eigen::Matrix3d getEvecs() const
Get the eigen vectors of the voxel covariance.
Definition: voxel_grid_covariance.h:138
pcl::KdTreeFLANN< PointT >
pcl::VoxelGridCovariance
A searchable voxel strucure containing the mean and covariance of the data.
Definition: voxel_grid_covariance.h:55
pcl::VoxelGridCovariance::Leaf::evals_
Eigen::Vector3d evals_
Eigen values of voxel covariance matrix.
Definition: voxel_grid_covariance.h:183
pcl::VoxelGrid::min_b_
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier.
Definition: voxel_grid.h:470
pcl::VoxelGridCovariance::Leaf::getEvals
Eigen::Vector3d getEvals() const
Get the eigen values of the voxel covariance.
Definition: voxel_grid_covariance.h:148
pcl::VoxelGridCovariance::Leaf::evecs_
Eigen::Matrix3d evecs_
Eigen vectors of voxel covariance matrix.
Definition: voxel_grid_covariance.h:180
pcl::VoxelGridCovariance::getFaceNeighborsAtPoint
int getFaceNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p and its facing voxels (up to 7 voxels).
Definition: voxel_grid_covariance.hpp:417
pcl::VoxelGridCovariance::Leaf::mean_
Eigen::Vector3d mean_
3D voxel centroid
Definition: voxel_grid_covariance.h:166
pcl::VoxelGridCovariance::searchable_
bool searchable_
Flag to determine if voxel structure is searchable.
Definition: voxel_grid_covariance.h:561
pcl::VoxelGridCovariance::Leaf::getCov
Eigen::Matrix3d getCov() const
Get the voxel covariance.
Definition: voxel_grid_covariance.h:110
pcl::Filter< PointTarget >::ConstPtr
shared_ptr< const Filter< PointTarget > > ConstPtr
Definition: filter.h:84
pcl::KdTreeFLANN::radiusSearch
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all the nearest neighbors of the query point in a given radius.
Definition: kdtree_flann.hpp:172
pcl::VoxelGridCovariance::Leaf::Leaf
Leaf()
Constructor.
Definition: voxel_grid_covariance.h:96
pcl::VoxelGridCovariance::min_covar_eigvalue_mult_
double min_covar_eigvalue_mult_
Minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
Definition: voxel_grid_covariance.h:567
pcl::VoxelGridCovariance::getNeighborhoodAtPoint
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.
Definition: voxel_grid_covariance.hpp:370
pcl::VoxelGridCovariance::getLeaf
LeafConstPtr getLeaf(Eigen::Vector3f &p)
Get the voxel containing point p.
Definition: voxel_grid_covariance.h:341
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::VoxelGrid::divb_mul_
Eigen::Vector4i divb_mul_
Definition: voxel_grid.h:470
pcl::VoxelGridCovariance::leaves_
std::map< std::size_t, Leaf > leaves_
Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of poin...
Definition: voxel_grid_covariance.h:570
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::VoxelGridCovariance::setMinPointPerVoxel
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 ...
Definition: voxel_grid_covariance.h:218
pcl::VoxelGridCovariance::getVoxelAtPoint
int getVoxelAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p.
Definition: voxel_grid_covariance.hpp:410
pcl::VoxelGridCovariance::getMinPointPerVoxel
int getMinPointPerVoxel()
Get the minimum number of points required for a cell to be used.
Definition: voxel_grid_covariance.h:235
pcl::VoxelGridCovariance::filter
void filter(PointCloud &output, bool searchable=false)
Filter cloud and initializes voxel structure.
Definition: voxel_grid_covariance.h:263
pcl::Filter::filter_name_
std::string filter_name_
The filter name.
Definition: filter.h:158
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:407
pcl::VoxelGridCovariance::Leaf::getInverseCov
Eigen::Matrix3d getInverseCov() const
Get the inverse of the voxel covariance.
Definition: voxel_grid_covariance.h:119
pcl::VoxelGridCovariance::Leaf::getPointCount
int getPointCount() const
Get the number of points contained by this voxel.
Definition: voxel_grid_covariance.h:157
pcl::VoxelGridCovariance::Leaf::cov_
Eigen::Matrix3d cov_
Voxel covariance matrix.
Definition: voxel_grid_covariance.h:174
pcl::Filter::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: filter.h:174
pcl::VoxelGridCovariance::Leaf::nr_points
int nr_points
Number of points contained by voxel.
Definition: voxel_grid_covariance.h:163
pcl::VoxelGrid::leaf_size_
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition: voxel_grid.h:455
pcl::VoxelGridCovariance::nearestKSearch
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.
Definition: voxel_grid_covariance.h:444
pcl::VoxelGridCovariance::Leaf
Simple structure to hold a centroid, covarince and the number of points in a leaf.
Definition: voxel_grid_covariance.h:91
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:408
pcl::VoxelGridCovariance::nearestKSearch
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.
Definition: voxel_grid_covariance.h:484
pcl::VoxelGrid::max_b_
Eigen::Vector4i max_b_
Definition: voxel_grid.h:470
pcl::VoxelGridCovariance::min_points_per_voxel_
int min_points_per_voxel_
Minimum points contained with in a voxel to allow it to be usable.
Definition: voxel_grid_covariance.h:564
pcl::VoxelGridCovariance::setCovEigValueInflationRatio
void setCovEigValueInflationRatio(double min_covar_eigvalue_mult)
Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
Definition: voxel_grid_covariance.h:244
pcl::KdTreeFLANN::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition: kdtree_flann.hpp:92
pcl::VoxelGridCovariance::getAllNeighborsAtPoint
int getAllNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get all 3x3x3 neighbor voxels of p (up to 27 voxels).
Definition: voxel_grid_covariance.hpp:433