Point Cloud Library (PCL) 1.12.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/voxel_grid.h>
41#include <map>
42#include <pcl/point_types.h>
43#include <pcl/kdtree/kdtree_flann.h>
44
45namespace 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;
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
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
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_ (),
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 {
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
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 #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
shared_ptr< Filter< PointT > > Ptr
Definition: filter.h:83
shared_ptr< const Filter< PointT > > ConstPtr
Definition: filter.h:84
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: filter.h:174
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).
const std::map< std::size_t, Leaf > & getLeaves()
Get the leaf structure map.
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.
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: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
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.