Point Cloud Library (PCL) 1.15.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 structure 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 mean_ (Eigen::Vector3d::Zero ()),
98 cov_ (Eigen::Matrix3d::Zero ()),
99 icov_ (Eigen::Matrix3d::Zero ()),
100 evecs_ (Eigen::Matrix3d::Identity ()),
101 evals_ (Eigen::Vector3d::Zero ())
102 {
103 }
104
105 /** \brief Get the voxel covariance.
106 * \return covariance matrix
107 */
108 Eigen::Matrix3d
109 getCov () const
110 {
111 return (cov_);
112 }
113
114 /** \brief Get the inverse of the voxel covariance.
115 * \return inverse covariance matrix
116 */
117 Eigen::Matrix3d
119 {
120 return (icov_);
121 }
122
123 /** \brief Get the voxel centroid.
124 * \return centroid
125 */
126 Eigen::Vector3d
127 getMean () const
128 {
129 return (mean_);
130 }
131
132 /** \brief Get the eigen vectors of the voxel covariance.
133 * \note Order corresponds with \ref getEvals
134 * \return matrix whose columns contain eigen vectors
135 */
136 Eigen::Matrix3d
137 getEvecs () const
138 {
139 return (evecs_);
140 }
141
142 /** \brief Get the eigen values of the voxel covariance.
143 * \note Order corresponds with \ref getEvecs
144 * \return vector of eigen values
145 */
146 Eigen::Vector3d
147 getEvals () const
148 {
149 return (evals_);
150 }
151
152 /** \brief Get the number of points contained by this voxel.
153 * \return number of points
154 */
155 int
157 {
158 return (nr_points);
159 }
160
161 /** \brief Number of points contained by voxel */
162 int nr_points{0};
163
164 /** \brief 3D voxel centroid */
165 Eigen::Vector3d mean_;
166
167 /** \brief Nd voxel centroid
168 * \note Differs from \ref mean_ when color data is used
169 */
170 Eigen::VectorXf centroid;
171
172 /** \brief Voxel covariance matrix */
173 Eigen::Matrix3d cov_;
174
175 /** \brief Inverse of voxel covariance matrix */
176 Eigen::Matrix3d icov_;
177
178 /** \brief Eigen vectors of voxel covariance matrix */
179 Eigen::Matrix3d evecs_;
180
181 /** \brief Eigen values of voxel covariance matrix */
182 Eigen::Vector3d evals_;
183
184 };
185
186 /** \brief Pointer to VoxelGridCovariance leaf structure */
187 using LeafPtr = Leaf *;
188
189 /** \brief Const pointer to VoxelGridCovariance leaf structure */
190 using LeafConstPtr = const Leaf *;
191
192 public:
193
194 /** \brief Constructor.
195 * Sets \ref leaf_size_ to 0 and \ref searchable_ to false.
196 */
198 leaves_ (),
200 kdtree_ ()
201 {
202 downsample_all_data_ = false;
203 save_leaf_layout_ = false;
204 leaf_size_.setZero ();
205 min_b_.setZero ();
206 max_b_.setZero ();
207 filter_name_ = "VoxelGridCovariance";
208 }
209
210 /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation).
211 * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
212 */
213 inline void
214 setMinPointPerVoxel (int min_points_per_voxel)
215 {
216 if(min_points_per_voxel > 2)
217 {
218 min_points_per_voxel_ = min_points_per_voxel;
219 }
220 else
221 {
222 PCL_WARN ("[%s::setMinPointPerVoxel] Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3\n", this->getClassName ().c_str ());
224 }
225 }
226
227 /** \brief Get the minimum number of points required for a cell to be used.
228 * \return the minimum number of points for required for a voxel to be used
229 */
230 inline int
232 {
234 }
235
236 /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
237 * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues
238 */
239 inline void
240 setCovEigValueInflationRatio (double min_covar_eigvalue_mult)
241 {
242 min_covar_eigvalue_mult_ = min_covar_eigvalue_mult;
243 }
244
245 /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
246 * \return the minimum allowable ratio between eigenvalues
247 */
248 inline double
253
254 /** \brief Filter cloud and initializes voxel structure.
255 * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
256 * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
257 */
258 inline void
259 filter (PointCloud &output, bool searchable = false)
260 {
261 searchable_ = searchable;
262 applyFilter (output);
263
265
266 if (searchable_)
267 {
268 if (voxel_centroids_->empty ()) {
269 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 ());
270 searchable_ = false;
271 } else {
272 // Initiates kdtree of the centroids of voxels containing a sufficient number of points
273 kdtree_.setInputCloud (voxel_centroids_);
274 }
275 }
276 }
277
278 /** \brief Initializes voxel structure.
279 * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
280 */
281 inline void
282 filter (bool searchable = false)
283 {
284 searchable_ = searchable;
287
288 if (searchable_)
289 {
290 if (voxel_centroids_->empty ()) {
291 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 ());
292 searchable_ = false;
293 } else {
294 // Initiates kdtree of the centroids of voxels containing a sufficient number of points
295 kdtree_.setInputCloud (voxel_centroids_);
296 }
297 }
298 }
299
300 /** \brief Get the voxel containing point p.
301 * \param[in] index the index of the leaf structure node
302 * \return const pointer to leaf structure
303 */
304 inline LeafConstPtr
305 getLeaf (int index)
306 {
307 auto leaf_iter = leaves_.find (index);
308 if (leaf_iter != leaves_.end ())
309 {
310 LeafConstPtr ret (&(leaf_iter->second));
311 return ret;
312 }
313 return nullptr;
314 }
315
316 /** \brief Get the voxel containing point p.
317 * \param[in] p the point to get the leaf structure at
318 * \return const pointer to leaf structure
319 */
320 inline LeafConstPtr
322 {
323 // Generate index associated with p
324 int ijk0 = static_cast<int> (std::floor (p.x * inverse_leaf_size_[0]) - min_b_[0]);
325 int ijk1 = static_cast<int> (std::floor (p.y * inverse_leaf_size_[1]) - min_b_[1]);
326 int ijk2 = static_cast<int> (std::floor (p.z * inverse_leaf_size_[2]) - min_b_[2]);
327
328 // Compute the centroid leaf index
329 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
330
331 // Find leaf associated with index
332 auto leaf_iter = leaves_.find (idx);
333 if (leaf_iter != leaves_.end ())
334 {
335 // If such a leaf exists return the pointer to the leaf structure
336 LeafConstPtr ret (&(leaf_iter->second));
337 return ret;
338 }
339 return nullptr;
340 }
341
342 /** \brief Get the voxel containing point p.
343 * \param[in] p the point to get the leaf structure at
344 * \return const pointer to leaf structure
345 */
346 inline LeafConstPtr
347 getLeaf (Eigen::Vector3f &p)
348 {
349 // Generate index associated with p
350 int ijk0 = static_cast<int> (std::floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]);
351 int ijk1 = static_cast<int> (std::floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]);
352 int ijk2 = static_cast<int> (std::floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]);
353
354 // Compute the centroid leaf index
355 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
356
357 // Find leaf associated with index
358 auto leaf_iter = leaves_.find (idx);
359 if (leaf_iter != leaves_.end ())
360 {
361 // If such a leaf exists return the pointer to the leaf structure
362 LeafConstPtr ret (&(leaf_iter->second));
363 return ret;
364 }
365 return nullptr;
366
367 }
368
369 /** \brief Get the voxels surrounding point p designated by \p relative_coordinates.
370 * \note Only voxels containing a sufficient number of points are used.
371 * \param[in] relative_coordinates 3xN matrix that represents relative coordinates of N neighboring voxels with respect to the center voxel
372 * \param[in] reference_point the point to get the leaf structure at
373 * \param[out] neighbors
374 * \return number of neighbors found
375 */
376 int
377 getNeighborhoodAtPoint (const Eigen::Matrix<int, 3, Eigen::Dynamic>& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
378
379 /** \brief Get the voxels surrounding point p, not including the voxel containing point p.
380 * \note Only voxels containing a sufficient number of points are used.
381 * \param[in] reference_point the point to get the leaf structure at
382 * \param[out] neighbors
383 * \return number of neighbors found (up to 26)
384 */
385 int
386 getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
387
388 /** \brief Get the voxel at p.
389 * \note Only voxels containing a sufficient number of points are used.
390 * \param[in] reference_point the point to get the leaf structure at
391 * \param[out] neighbors
392 * \return number of neighbors found (up to 1)
393 */
394 int
395 getVoxelAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
396
397 /** \brief Get the voxel at p and its facing voxels (up to 7 voxels).
398 * \note Only voxels containing a sufficient number of points are used.
399 * \param[in] reference_point the point to get the leaf structure at
400 * \param[out] neighbors
401 * \return number of neighbors found (up to 7)
402 */
403 int
404 getFaceNeighborsAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
405
406 /** \brief Get all 3x3x3 neighbor voxels of p (up to 27 voxels).
407 * \note Only voxels containing a sufficient number of points are used.
408 * \param[in] reference_point the point to get the leaf structure at
409 * \param[out] neighbors
410 * \return number of neighbors found (up to 27)
411 */
412 int
413 getAllNeighborsAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const;
414
415 /** \brief Get the leaf structure map
416 * \return a map contataining all leaves
417 */
418 inline const std::map<std::size_t, Leaf>&
420 {
421 return leaves_;
422 }
423
424 /** \brief Get a pointcloud containing the voxel centroids
425 * \note Only voxels containing a sufficient number of points are used.
426 * \return a map contataining all leaves
427 */
428 inline PointCloudPtr
430 {
431 return voxel_centroids_;
432 }
433
434
435 /** \brief Get a cloud to visualize each voxels normal distribution.
436 * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
437 * \param[in] pnt_per_cell how many points should be generated for each cell
438 */
439 void
440 getDisplayCloud (pcl::PointCloud<PointXYZ>& cell_cloud, int pnt_per_cell = 1000) const;
441
442 /** \brief Search for the k-nearest occupied voxels for the given query point.
443 * \note Only voxels containing a sufficient number of points are used.
444 * \param[in] point the given query point
445 * \param[in] k the number of neighbors to search for
446 * \param[out] k_leaves the resultant leaves of the neighboring points
447 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
448 * \return number of neighbors found
449 */
450 int
451 nearestKSearch (const PointT &point, int k,
452 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
453 {
454 k_leaves.clear ();
455
456 // Check if kdtree has been built
457 if (!searchable_)
458 {
459 PCL_WARN ("[%s::nearestKSearch] Not Searchable\n", this->getClassName ().c_str ());
460 return 0;
461 }
462
463 // Find k-nearest neighbors in the occupied voxel centroid cloud
464 Indices k_indices (k);
465 k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
466
467 // Find leaves corresponding to neighbors
468 k_leaves.reserve (k);
469 for (const auto &k_index : k_indices)
470 {
471 auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
472 if (voxel == leaves_.end()) {
473 continue;
474 }
475
476 k_leaves.push_back(&voxel->second);
477 }
478 return k_leaves.size();
479 }
480
481 /** \brief Search for the k-nearest occupied voxels for the given query point.
482 * \note Only voxels containing a sufficient number of points are used.
483 * \param[in] cloud the given query point
484 * \param[in] index the index
485 * \param[in] k the number of neighbors to search for
486 * \param[out] k_leaves the resultant leaves of the neighboring points
487 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
488 * \return number of neighbors found
489 */
490 inline int
491 nearestKSearch (const PointCloud &cloud, int index, int k,
492 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) const
493 {
494 if (index >= static_cast<int> (cloud.size ()) || index < 0)
495 return (0);
496 return (nearestKSearch (cloud[index], k, k_leaves, k_sqr_distances));
497 }
498
499
500 /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
501 * \note Only voxels containing a sufficient number of points are used.
502 * \param[in] point the given query point
503 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
504 * \param[out] k_leaves the resultant leaves of the neighboring points
505 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
506 * \param[in] max_nn
507 * \return number of neighbors found
508 */
509 int
510 radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
511 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
512 {
513 k_leaves.clear ();
514
515 // Check if kdtree has been built
516 if (!searchable_)
517 {
518 PCL_WARN ("[%s::radiusSearch] Not Searchable\n", this->getClassName ().c_str ());
519 return 0;
520 }
521
522 // Find neighbors within radius in the occupied voxel centroid cloud
523 Indices k_indices;
524 const int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
525
526 // Find leaves corresponding to neighbors
527 k_leaves.reserve (k);
528 for (const auto &k_index : k_indices)
529 {
530 const auto voxel = leaves_.find(voxel_centroids_leaf_indices_[k_index]);
531 if(voxel == leaves_.end()) {
532 continue;
533 }
534
535 k_leaves.push_back(&voxel->second);
536 }
537 return k_leaves.size();
538 }
539
540 /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
541 * \note Only voxels containing a sufficient number of points are used.
542 * \param[in] cloud the given query point
543 * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
544 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
545 * \param[out] k_leaves the resultant leaves of the neighboring points
546 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
547 * \param[in] max_nn
548 * \return number of neighbors found
549 */
550 inline int
551 radiusSearch (const PointCloud &cloud, int index, double radius,
552 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
553 unsigned int max_nn = 0) const
554 {
555 if (index >= static_cast<int> (cloud.size ()) || index < 0)
556 return (0);
557 return (radiusSearch (cloud[index], radius, k_leaves, k_sqr_distances, max_nn));
558 }
559
560 protected:
561
562 /** \brief Filter cloud and initializes voxel structure.
563 * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
564 */
565 void applyFilter (PointCloud &output) override;
566
567 /** \brief Flag to determine if voxel structure is searchable. */
568 bool searchable_{true};
569
570 /** \brief Minimum points contained with in a voxel to allow it to be usable. */
572
573 /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
575
576 /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
577 std::map<std::size_t, Leaf> leaves_;
578
579 /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */
581
582 /** \brief Indices of leaf structures associated with each point in \ref voxel_centroids_ (used for searching). */
584
585 /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
587 };
588}
589
590#ifdef PCL_NO_PRECOMPILE
591#include <pcl/filters/impl/voxel_grid_covariance.hpp>
592#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
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
A searchable voxel structure 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 getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud, int pnt_per_cell=1000) const
Get a cloud to visualize each voxels normal distribution.
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 structures associated with each point in voxel_centroids_ (used for searching).
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:210
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition voxel_grid.h:489
Eigen::Vector4i max_b_
Definition voxel_grid.h:498
std::vector< int > leaf_layout_
The leaf layout information for fast access to cells relative to current position.
Definition voxel_grid.h:495
Eigen::Vector4i divb_mul_
Definition voxel_grid.h:498
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
Definition voxel_grid.h:492
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:510
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier.
Definition voxel_grid.h:498
double filter_limit_max_
The maximum allowed filter value a point will be considered from.
Definition voxel_grid.h:507
std::string filter_field_name_
The desired user filter field name.
Definition voxel_grid.h:501
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition voxel_grid.h:483
double filter_limit_min_
The minimum allowed filter value a point will be considered from.
Definition voxel_grid.h:504
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition voxel_grid.h:486
Eigen::Vector4i div_b_
Definition voxel_grid.h:498
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.