Point Cloud Library (PCL) 1.13.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::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
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
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 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: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
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.