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