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