Point Cloud Library (PCL)  1.11.1-dev
voxel_grid_covariance.hpp
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 #ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_H_
39 #define PCL_VOXEL_GRID_COVARIANCE_IMPL_H_
40 
41 #include <pcl/common/common.h>
42 #include <pcl/filters/boost.h>
43 #include <pcl/filters/voxel_grid_covariance.h>
44 #include <Eigen/Dense>
45 #include <Eigen/Cholesky>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////
48 template<typename PointT> void
50 {
51  voxel_centroids_leaf_indices_.clear ();
52 
53  // Has the input dataset been set already?
54  if (!input_)
55  {
56  PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
57  output.width = output.height = 0;
58  output.points.clear ();
59  return;
60  }
61 
62  // Copy the header (and thus the frame_id) + allocate enough space for points
63  output.height = 1; // downsampling breaks the organized structure
64  output.is_dense = true; // we filter out invalid points
65  output.points.clear ();
66 
67  Eigen::Vector4f min_p, max_p;
68  // Get the minimum and maximum dimensions
69  if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
70  getMinMax3D<PointT> (input_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
71  else
72  getMinMax3D<PointT> (*input_, min_p, max_p);
73 
74  // Check that the leaf size is not too small, given the size of the data
75  std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
76  std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
77  std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
78 
79  if((dx*dy*dz) > std::numeric_limits<std::int32_t>::max())
80  {
81  PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
82  output.clear();
83  return;
84  }
85 
86  // Compute the minimum and maximum bounding box values
87  min_b_[0] = static_cast<int> (std::floor (min_p[0] * inverse_leaf_size_[0]));
88  max_b_[0] = static_cast<int> (std::floor (max_p[0] * inverse_leaf_size_[0]));
89  min_b_[1] = static_cast<int> (std::floor (min_p[1] * inverse_leaf_size_[1]));
90  max_b_[1] = static_cast<int> (std::floor (max_p[1] * inverse_leaf_size_[1]));
91  min_b_[2] = static_cast<int> (std::floor (min_p[2] * inverse_leaf_size_[2]));
92  max_b_[2] = static_cast<int> (std::floor (max_p[2] * inverse_leaf_size_[2]));
93 
94  // Compute the number of divisions needed along all axis
95  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
96  div_b_[3] = 0;
97 
98  // Clear the leaves
99  leaves_.clear ();
100 
101  // Set up the division multiplier
102  divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
103 
104  int centroid_size = 4;
105 
106  if (downsample_all_data_)
107  centroid_size = boost::mpl::size<FieldList>::value;
108 
109  // ---[ RGB special case
110  std::vector<pcl::PCLPointField> fields;
111  int rgba_index = -1;
112  rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
113  if (rgba_index == -1)
114  rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
115  if (rgba_index >= 0)
116  {
117  rgba_index = fields[rgba_index].offset;
118  centroid_size += 4;
119  }
120 
121  // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
122  if (!filter_field_name_.empty ())
123  {
124  // Get the distance field index
125  std::vector<pcl::PCLPointField> fields;
126  int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
127  if (distance_idx == -1)
128  PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
129 
130  // First pass: go over all points and insert them into the right leaf
131  for (const auto& point: *input_)
132  {
133  if (!input_->is_dense)
134  // Check if the point is invalid
135  if (!isXYZFinite (point))
136  continue;
137 
138  // Get the distance value
139  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&point);
140  float distance_value = 0;
141  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
142 
143  if (filter_limit_negative_)
144  {
145  // Use a threshold for cutting out points which inside the interval
146  if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
147  continue;
148  }
149  else
150  {
151  // Use a threshold for cutting out points which are too close/far away
152  if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
153  continue;
154  }
155 
156  // Compute the centroid leaf index
157  const Eigen::Vector4i ijk =
158  Eigen::floor(point.getArray4fMap() * inverse_leaf_size_.array())
159  .template cast<int>();
160  // divb_mul_[3] = 0 by assignment
161  int idx = (ijk - min_b_).dot(divb_mul_);
162 
163  Leaf& leaf = leaves_[idx];
164  if (leaf.nr_points == 0)
165  {
166  leaf.centroid.resize (centroid_size);
167  leaf.centroid.setZero ();
168  }
169 
170  Eigen::Vector3d pt3d = point.getVector3fMap().template cast<double>();
171  // Accumulate point sum for centroid calculation
172  leaf.mean_ += pt3d;
173  // Accumulate x*xT for single pass covariance calculation
174  leaf.cov_ += pt3d * pt3d.transpose ();
175 
176  // Do we need to process all the fields?
177  if (!downsample_all_data_)
178  {
179  leaf.centroid.template head<3> () += point.getVector3fMap();
180  }
181  else
182  {
183  // Copy all the fields
184  Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
185  pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (point, centroid));
186  // ---[ RGB special case
187  if (rgba_index >= 0)
188  {
189  // Fill r/g/b data, assuming that the order is BGRA
190  const pcl::RGB& rgb = *reinterpret_cast<const RGB*> (reinterpret_cast<const char*> (&point) + rgba_index);
191  centroid[centroid_size - 4] = rgb.a;
192  centroid[centroid_size - 3] = rgb.r;
193  centroid[centroid_size - 2] = rgb.g;
194  centroid[centroid_size - 1] = rgb.b;
195  }
196  leaf.centroid += centroid;
197  }
198  ++leaf.nr_points;
199  }
200  }
201  // No distance filtering, process all data
202  else
203  {
204  // First pass: go over all points and insert them into the right leaf
205  for (const auto& point: *input_)
206  {
207  if (!input_->is_dense)
208  // Check if the point is invalid
209  if (!isXYZFinite (point))
210  continue;
211 
212  // Compute the centroid leaf index
213  const Eigen::Vector4i ijk =
214  Eigen::floor(point.getArray4fMap() * inverse_leaf_size_.array())
215  .template cast<int>();
216  // divb_mul_[3] = 0 by assignment
217  int idx = (ijk - min_b_).dot(divb_mul_);
218 
219  Leaf& leaf = leaves_[idx];
220  if (leaf.nr_points == 0)
221  {
222  leaf.centroid.resize (centroid_size);
223  leaf.centroid.setZero ();
224  }
225 
226  Eigen::Vector3d pt3d = point.getVector3fMap().template cast<double>();
227  // Accumulate point sum for centroid calculation
228  leaf.mean_ += pt3d;
229  // Accumulate x*xT for single pass covariance calculation
230  leaf.cov_ += pt3d * pt3d.transpose ();
231 
232  // Do we need to process all the fields?
233  if (!downsample_all_data_)
234  {
235  leaf.centroid.template head<3> () += point.getVector3fMap();
236  }
237  else
238  {
239  // Copy all the fields
240  Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
241  pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (point, centroid));
242  // ---[ RGB special case
243  if (rgba_index >= 0)
244  {
245  // Fill r/g/b data, assuming that the order is BGRA
246  const pcl::RGB& rgb = *reinterpret_cast<const RGB*> (reinterpret_cast<const char*> (&point) + rgba_index);
247  centroid[centroid_size - 4] = rgb.a;
248  centroid[centroid_size - 3] = rgb.r;
249  centroid[centroid_size - 2] = rgb.g;
250  centroid[centroid_size - 1] = rgb.b;
251  }
252  leaf.centroid += centroid;
253  }
254  ++leaf.nr_points;
255  }
256  }
257 
258  // Second pass: go over all leaves and compute centroids and covariance matrices
259  output.points.reserve (leaves_.size ());
260  if (searchable_)
261  voxel_centroids_leaf_indices_.reserve (leaves_.size ());
262  int cp = 0;
263  if (save_leaf_layout_)
264  leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1);
265 
266  // Eigen values and vectors calculated to prevent near singluar matrices
267  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver;
268  Eigen::Matrix3d eigen_val;
269  Eigen::Vector3d pt_sum;
270 
271  // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max eigen value.
272  double min_covar_eigvalue;
273 
274  for (typename std::map<std::size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
275  {
276 
277  // Normalize the centroid
278  Leaf& leaf = it->second;
279 
280  // Normalize the centroid
281  leaf.centroid /= static_cast<float> (leaf.nr_points);
282  // Point sum used for single pass covariance calculation
283  pt_sum = leaf.mean_;
284  // Normalize mean
285  leaf.mean_ /= leaf.nr_points;
286 
287  // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and output clouds.
288  // Points with less than the minimum points will have a can not be accuratly approximated using a normal distribution.
289  if (leaf.nr_points >= min_points_per_voxel_)
290  {
291  if (save_leaf_layout_)
292  leaf_layout_[it->first] = cp++;
293 
294  output.push_back (PointT ());
295 
296  // Do we need to process all the fields?
297  if (!downsample_all_data_)
298  {
299  output.points.back ().x = leaf.centroid[0];
300  output.points.back ().y = leaf.centroid[1];
301  output.points.back ().z = leaf.centroid[2];
302  }
303  else
304  {
305  pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor<PointT> (leaf.centroid, output.back ()));
306  // ---[ RGB special case
307  if (rgba_index >= 0)
308  {
309  pcl::RGB& rgb = *reinterpret_cast<RGB*> (reinterpret_cast<char*> (&output.points.back ()) + rgba_index);
310  rgb.a = leaf.centroid[centroid_size - 4];
311  rgb.r = leaf.centroid[centroid_size - 3];
312  rgb.g = leaf.centroid[centroid_size - 2];
313  rgb.b = leaf.centroid[centroid_size - 1];
314  }
315  }
316 
317  // Stores the voxel indice for fast access searching
318  if (searchable_)
319  voxel_centroids_leaf_indices_.push_back (static_cast<int> (it->first));
320 
321  // Single pass covariance calculation
322  leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose ();
323  leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points;
324 
325  //Normalize Eigen Val such that max no more than 100x min.
326  eigensolver.compute (leaf.cov_);
327  eigen_val = eigensolver.eigenvalues ().asDiagonal ();
328  leaf.evecs_ = eigensolver.eigenvectors ();
329 
330  if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0)
331  {
332  leaf.nr_points = -1;
333  continue;
334  }
335 
336  // Avoids matrices near singularities (eq 6.11)[Magnusson 2009]
337 
338  min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2);
339  if (eigen_val (0, 0) < min_covar_eigvalue)
340  {
341  eigen_val (0, 0) = min_covar_eigvalue;
342 
343  if (eigen_val (1, 1) < min_covar_eigvalue)
344  {
345  eigen_val (1, 1) = min_covar_eigvalue;
346  }
347 
348  leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse ();
349  }
350  leaf.evals_ = eigen_val.diagonal ();
351 
352  leaf.icov_ = leaf.cov_.inverse ();
353  if (leaf.icov_.maxCoeff () == std::numeric_limits<float>::infinity ( )
354  || leaf.icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) )
355  {
356  leaf.nr_points = -1;
357  }
358 
359  }
360  }
361 
362  output.width = output.size ();
363 }
364 
365 //////////////////////////////////////////////////////////////////////////////////////////
366 template<typename PointT> int
367 pcl::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint (const Eigen::Matrix<int, 3, Eigen::Dynamic>& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
368 {
369  neighbors.clear ();
370 
371  // Find displacement coordinates
372  Eigen::Vector4i ijk = Eigen::floor(reference_point.getArray4fMap() * inverse_leaf_size_).template cast<int>();
373  ijk[3] = 0;
374  const Eigen::Array4i diff2min = min_b_ - ijk;
375  const Eigen::Array4i diff2max = max_b_ - ijk;
376  neighbors.reserve (relative_coordinates.cols ());
377 
378  // Check each neighbor to see if it is occupied and contains sufficient points
379  for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++)
380  {
381  const Eigen::Vector4i displacement = (Eigen::Vector4i () << relative_coordinates.col (ni), 0).finished ();
382  // Checking if the specified cell is in the grid
383  if ((diff2min <= displacement.array ()).all () && (diff2max >= displacement.array ()).all ())
384  {
385  const auto leaf_iter = leaves_.find (((ijk + displacement - min_b_).dot (divb_mul_)));
386  if (leaf_iter != leaves_.end () && leaf_iter->second.nr_points >= min_points_per_voxel_)
387  {
388  LeafConstPtr leaf = &(leaf_iter->second);
389  neighbors.push_back (leaf);
390  }
391  }
392  }
393 
394  return static_cast<int> (neighbors.size());
395 }
396 
397 //////////////////////////////////////////////////////////////////////////////////////////
398 template<typename PointT> int
399 pcl::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
400 {
401  Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices();
402  return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
403 }
404 
405 //////////////////////////////////////////////////////////////////////////////////////////
406 template<typename PointT> int
407 pcl::VoxelGridCovariance<PointT>::getVoxelAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
408 {
409  return getNeighborhoodAtPoint(Eigen::Matrix<int, 3, Eigen::Dynamic>::Zero(3,1), reference_point, neighbors);
410 }
411 
412 //////////////////////////////////////////////////////////////////////////////////////////
413 template<typename PointT> int
414 pcl::VoxelGridCovariance<PointT>::getFaceNeighborsAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
415 {
416  Eigen::Matrix<int, 3, Eigen::Dynamic> relative_coordinates(3, 7);
417  relative_coordinates.setZero();
418  relative_coordinates(0, 1) = 1;
419  relative_coordinates(0, 2) = -1;
420  relative_coordinates(1, 3) = 1;
421  relative_coordinates(1, 4) = -1;
422  relative_coordinates(2, 5) = 1;
423  relative_coordinates(2, 6) = -1;
424 
425  return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
426 }
427 
428 //////////////////////////////////////////////////////////////////////////////////////////
429 template<typename PointT> int
430 pcl::VoxelGridCovariance<PointT>::getAllNeighborsAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
431 {
432  Eigen::Matrix<int, 3, Eigen::Dynamic> relative_coordinates(3, 27);
433  relative_coordinates.col(0).setZero();
434  relative_coordinates.rightCols(26) = pcl::getAllNeighborCellIndices();
435 
436  return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
437 }
438 
439 //////////////////////////////////////////////////////////////////////////////////////////
440 template<typename PointT> void
442 {
443  cell_cloud.clear ();
444 
445  int pnt_per_cell = 1000;
446  boost::mt19937 rng;
447  boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
448  boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
449 
450  Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
451  Eigen::Matrix3d cholesky_decomp;
452  Eigen::Vector3d cell_mean;
453  Eigen::Vector3d rand_point;
454  Eigen::Vector3d dist_point;
455 
456  // Generate points for each occupied voxel with sufficient points.
457  for (typename std::map<std::size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
458  {
459  Leaf& leaf = it->second;
460 
461  if (leaf.nr_points >= min_points_per_voxel_)
462  {
463  cell_mean = leaf.mean_;
464  llt_of_cov.compute (leaf.cov_);
465  cholesky_decomp = llt_of_cov.matrixL ();
466 
467  // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix
468  for (int i = 0; i < pnt_per_cell; i++)
469  {
470  rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ());
471  dist_point = cell_mean + cholesky_decomp * rand_point;
472  cell_cloud.push_back (PointXYZ (static_cast<float> (dist_point (0)), static_cast<float> (dist_point (1)), static_cast<float> (dist_point (2))));
473  }
474  }
475  }
476 }
477 
478 #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>;
479 
480 #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_
pcl::NdCopyEigenPointFunctor
Helper functor structure for copying data between an Eigen type and a PointT.
Definition: point_cloud.h:76
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
common.h
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
pcl::VoxelGridCovariance::applyFilter
void applyFilter(PointCloud &output) override
Filter cloud and initializes voxel structure.
Definition: voxel_grid_covariance.hpp:49
pcl::VoxelGridCovariance< PointTarget >::LeafConstPtr
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
Definition: voxel_grid_covariance.h:192
pcl::isXYZFinite
constexpr bool isXYZFinite(const PointT &) noexcept
Definition: point_tests.h:115
pcl::PointCloud< PointTarget >
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::getAllNeighborCellIndices
Eigen::MatrixXi getAllNeighborCellIndices()
Get the relative cell indices of all the 26 neighbors.
Definition: voxel_grid.h:121
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
pcl::VoxelGridCovariance::getDisplayCloud
void getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud)
Get a cloud to visualize each voxels normal distribution.
Definition: voxel_grid_covariance.hpp:441
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition: point_types.hpp:300
pcl::int64_t
std::int64_t int64_t
Definition: types.h:61
pcl::VoxelGridCovariance::getFaceNeighborsAtPoint
int getFaceNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p and its facing voxels (up to 7 voxels).
Definition: voxel_grid_covariance.hpp:414
pcl::PointCloud::back
const PointT & back() const
Definition: point_cloud.h:516
pcl::RGB
A structure representing RGB color information.
Definition: point_types.hpp:345
pcl::VoxelGridCovariance::getNeighborhoodAtPoint
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.
Definition: voxel_grid_covariance.hpp:367
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:419
pcl::NdCopyPointEigenFunctor
Helper functor structure for copying data between an Eigen type and a PointT.
Definition: point_cloud.h:109
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:459
pcl::VoxelGridCovariance::getVoxelAtPoint
int getVoxelAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p.
Definition: voxel_grid_covariance.hpp:407
pcl::PointCloud::clear
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:691
pcl::uint8_t
std::uint8_t uint8_t
Definition: types.h:54
pcl::PointCloud::push_back
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:566
pcl::VoxelGridCovariance::getAllNeighborsAtPoint
int getAllNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get all 3x3x3 neighbor voxels of p (up to 27 voxels).
Definition: voxel_grid_covariance.hpp:430