Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
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
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.
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< PointCloud< PointT > > Ptr
A searchable voxel strucure containing the mean and covariance of the data.
void applyFilter(PointCloud &output) override
Filter cloud and initializes voxel structure.
shared_ptr< VoxelGrid< PointT > > Ptr
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.
typename Filter< PointT >::PointCloud PointCloud
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.
shared_ptr< const VoxelGrid< PointT > > ConstPtr
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.
typename PointCloud::ConstPtr PointCloudConstPtr
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.
typename pcl::traits::fieldList< PointT >::type FieldList
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:461
Eigen::Vector4i max_b_
Definition voxel_grid.h:470
std::vector< int > leaf_layout_
The leaf layout information for fast access to cells relative to current position.
Definition voxel_grid.h:467
Eigen::Vector4i divb_mul_
Definition voxel_grid.h:470
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
Definition voxel_grid.h:464
bool filter_limit_negative_
Set to true if we want to return the data outside (filter_limit_min_;filter_limit_max_).
Definition voxel_grid.h:482
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier.
Definition voxel_grid.h:470
double filter_limit_max_
The maximum allowed filter value a point will be considered from.
Definition voxel_grid.h:479
std::string filter_field_name_
The desired user filter field name.
Definition voxel_grid.h:473
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition voxel_grid.h:455
double filter_limit_min_
The minimum allowed filter value a point will be considered from.
Definition voxel_grid.h:476
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition voxel_grid.h:458
Eigen::Vector4i div_b_
Definition voxel_grid.h:470
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.