Point Cloud Library (PCL)  1.8.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  int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
76  int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
77  int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
78 
79  if((dx*dy*dz) > std::numeric_limits<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> (floor (min_p[0] * inverse_leaf_size_[0]));
88  max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
89  min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
90  max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
91  min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
92  max_b_[2] = static_cast<int> (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 (*input_, "rgb", fields);
113  if (rgba_index == -1)
114  rgba_index = pcl::getFieldIndex (*input_, "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 (*input_, 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 (size_t cp = 0; cp < input_->points.size (); ++cp)
132  {
133  if (!input_->is_dense)
134  // Check if the point is invalid
135  if (!pcl_isfinite (input_->points[cp].x) ||
136  !pcl_isfinite (input_->points[cp].y) ||
137  !pcl_isfinite (input_->points[cp].z))
138  continue;
139 
140  // Get the distance value
141  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&input_->points[cp]);
142  float distance_value = 0;
143  memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
144 
145  if (filter_limit_negative_)
146  {
147  // Use a threshold for cutting out points which inside the interval
148  if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
149  continue;
150  }
151  else
152  {
153  // Use a threshold for cutting out points which are too close/far away
154  if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
155  continue;
156  }
157 
158  int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
159  int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
160  int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
161 
162  // Compute the centroid leaf index
163  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
164 
165  Leaf& leaf = leaves_[idx];
166  if (leaf.nr_points == 0)
167  {
168  leaf.centroid.resize (centroid_size);
169  leaf.centroid.setZero ();
170  }
171 
172  Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z);
173  // Accumulate point sum for centroid calculation
174  leaf.mean_ += pt3d;
175  // Accumulate x*xT for single pass covariance calculation
176  leaf.cov_ += pt3d * pt3d.transpose ();
177 
178  // Do we need to process all the fields?
179  if (!downsample_all_data_)
180  {
181  Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
182  leaf.centroid.template head<4> () += pt;
183  }
184  else
185  {
186  // Copy all the fields
187  Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
188  pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
189  // ---[ RGB special case
190  if (rgba_index >= 0)
191  {
192  // Fill r/g/b data, assuming that the order is BGRA
193  const pcl::RGB& rgb = *reinterpret_cast<const RGB*> (reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index);
194  centroid[centroid_size - 4] = rgb.a;
195  centroid[centroid_size - 3] = rgb.r;
196  centroid[centroid_size - 2] = rgb.g;
197  centroid[centroid_size - 1] = rgb.b;
198  }
199  leaf.centroid += centroid;
200  }
201  ++leaf.nr_points;
202  }
203  }
204  // No distance filtering, process all data
205  else
206  {
207  // First pass: go over all points and insert them into the right leaf
208  for (size_t cp = 0; cp < input_->points.size (); ++cp)
209  {
210  if (!input_->is_dense)
211  // Check if the point is invalid
212  if (!pcl_isfinite (input_->points[cp].x) ||
213  !pcl_isfinite (input_->points[cp].y) ||
214  !pcl_isfinite (input_->points[cp].z))
215  continue;
216 
217  int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
218  int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
219  int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
220 
221  // Compute the centroid leaf index
222  int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
223 
224  //int idx = (((input_->points[cp].getArray4fmap () * inverse_leaf_size_).template cast<int> ()).matrix () - min_b_).dot (divb_mul_);
225  Leaf& leaf = leaves_[idx];
226  if (leaf.nr_points == 0)
227  {
228  leaf.centroid.resize (centroid_size);
229  leaf.centroid.setZero ();
230  }
231 
232  Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z);
233  // Accumulate point sum for centroid calculation
234  leaf.mean_ += pt3d;
235  // Accumulate x*xT for single pass covariance calculation
236  leaf.cov_ += pt3d * pt3d.transpose ();
237 
238  // Do we need to process all the fields?
239  if (!downsample_all_data_)
240  {
241  Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
242  leaf.centroid.template head<4> () += pt;
243  }
244  else
245  {
246  // Copy all the fields
247  Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
248  pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
249  // ---[ RGB special case
250  if (rgba_index >= 0)
251  {
252  // Fill r/g/b data, assuming that the order is BGRA
253  const pcl::RGB& rgb = *reinterpret_cast<const RGB*> (reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index);
254  centroid[centroid_size - 4] = rgb.a;
255  centroid[centroid_size - 3] = rgb.r;
256  centroid[centroid_size - 2] = rgb.g;
257  centroid[centroid_size - 1] = rgb.b;
258  }
259  leaf.centroid += centroid;
260  }
261  ++leaf.nr_points;
262  }
263  }
264 
265  // Second pass: go over all leaves and compute centroids and covariance matrices
266  output.points.reserve (leaves_.size ());
267  if (searchable_)
268  voxel_centroids_leaf_indices_.reserve (leaves_.size ());
269  int cp = 0;
270  if (save_leaf_layout_)
271  leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1);
272 
273  // Eigen values and vectors calculated to prevent near singluar matrices
274  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver;
275  Eigen::Matrix3d eigen_val;
276  Eigen::Vector3d pt_sum;
277 
278  // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max eigen value.
279  double min_covar_eigvalue;
280 
281  for (typename std::map<size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
282  {
283 
284  // Normalize the centroid
285  Leaf& leaf = it->second;
286 
287  // Normalize the centroid
288  leaf.centroid /= static_cast<float> (leaf.nr_points);
289  // Point sum used for single pass covariance calculation
290  pt_sum = leaf.mean_;
291  // Normalize mean
292  leaf.mean_ /= leaf.nr_points;
293 
294  // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and output clouds.
295  // Points with less than the minimum points will have a can not be accuratly approximated using a normal distribution.
296  if (leaf.nr_points >= min_points_per_voxel_)
297  {
298  if (save_leaf_layout_)
299  leaf_layout_[it->first] = cp++;
300 
301  output.push_back (PointT ());
302 
303  // Do we need to process all the fields?
304  if (!downsample_all_data_)
305  {
306  output.points.back ().x = leaf.centroid[0];
307  output.points.back ().y = leaf.centroid[1];
308  output.points.back ().z = leaf.centroid[2];
309  }
310  else
311  {
312  pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor<PointT> (leaf.centroid, output.back ()));
313  // ---[ RGB special case
314  if (rgba_index >= 0)
315  {
316  pcl::RGB& rgb = *reinterpret_cast<RGB*> (reinterpret_cast<char*> (&output.points.back ()) + rgba_index);
317  rgb.a = leaf.centroid[centroid_size - 4];
318  rgb.r = leaf.centroid[centroid_size - 3];
319  rgb.g = leaf.centroid[centroid_size - 2];
320  rgb.b = leaf.centroid[centroid_size - 1];
321  }
322  }
323 
324  // Stores the voxel indice for fast access searching
325  if (searchable_)
326  voxel_centroids_leaf_indices_.push_back (static_cast<int> (it->first));
327 
328  // Single pass covariance calculation
329  leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose ();
330  leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points;
331 
332  //Normalize Eigen Val such that max no more than 100x min.
333  eigensolver.compute (leaf.cov_);
334  eigen_val = eigensolver.eigenvalues ().asDiagonal ();
335  leaf.evecs_ = eigensolver.eigenvectors ();
336 
337  if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0)
338  {
339  leaf.nr_points = -1;
340  continue;
341  }
342 
343  // Avoids matrices near singularities (eq 6.11)[Magnusson 2009]
344 
345  min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2);
346  if (eigen_val (0, 0) < min_covar_eigvalue)
347  {
348  eigen_val (0, 0) = min_covar_eigvalue;
349 
350  if (eigen_val (1, 1) < min_covar_eigvalue)
351  {
352  eigen_val (1, 1) = min_covar_eigvalue;
353  }
354 
355  leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse ();
356  }
357  leaf.evals_ = eigen_val.diagonal ();
358 
359  leaf.icov_ = leaf.cov_.inverse ();
360  if (leaf.icov_.maxCoeff () == std::numeric_limits<float>::infinity ( )
361  || leaf.icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) )
362  {
363  leaf.nr_points = -1;
364  }
365 
366  }
367  }
368 
369  output.width = static_cast<uint32_t> (output.points.size ());
370 }
371 
372 //////////////////////////////////////////////////////////////////////////////////////////
373 template<typename PointT> int
374 pcl::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors)
375 {
376  neighbors.clear ();
377 
378  // Find displacement coordinates
379  Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices ();
380  Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x / leaf_size_[0])),
381  static_cast<int> (floor (reference_point.y / leaf_size_[1])),
382  static_cast<int> (floor (reference_point.z / leaf_size_[2])), 0);
383  Eigen::Array4i diff2min = min_b_ - ijk;
384  Eigen::Array4i diff2max = max_b_ - ijk;
385  neighbors.reserve (relative_coordinates.cols ());
386 
387  // Check each neighbor to see if it is occupied and contains sufficient points
388  // Slower than radius search because needs to check 26 indices
389  for (int ni = 0; ni < relative_coordinates.cols (); ni++)
390  {
391  Eigen::Vector4i displacement = (Eigen::Vector4i () << relative_coordinates.col (ni), 0).finished ();
392  // Checking if the specified cell is in the grid
393  if ((diff2min <= displacement.array ()).all () && (diff2max >= displacement.array ()).all ())
394  {
395  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (((ijk + displacement - min_b_).dot (divb_mul_)));
396  if (leaf_iter != leaves_.end () && leaf_iter->second.nr_points >= min_points_per_voxel_)
397  {
398  LeafConstPtr leaf = &(leaf_iter->second);
399  neighbors.push_back (leaf);
400  }
401  }
402  }
403 
404  return (static_cast<int> (neighbors.size ()));
405 }
406 
407 //////////////////////////////////////////////////////////////////////////////////////////
408 template<typename PointT> void
410 {
411  cell_cloud.clear ();
412 
413  int pnt_per_cell = 1000;
414  boost::mt19937 rng;
415  boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
416  boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
417 
418  Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
419  Eigen::Matrix3d cholesky_decomp;
420  Eigen::Vector3d cell_mean;
421  Eigen::Vector3d rand_point;
422  Eigen::Vector3d dist_point;
423 
424  // Generate points for each occupied voxel with sufficient points.
425  for (typename std::map<size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
426  {
427  Leaf& leaf = it->second;
428 
429  if (leaf.nr_points >= min_points_per_voxel_)
430  {
431  cell_mean = leaf.mean_;
432  llt_of_cov.compute (leaf.cov_);
433  cholesky_decomp = llt_of_cov.matrixL ();
434 
435  // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix
436  for (int i = 0; i < pnt_per_cell; i++)
437  {
438  rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ());
439  dist_point = cell_mean + cholesky_decomp * rand_point;
440  cell_cloud.push_back (PointXYZ (static_cast<float> (dist_point (0)), static_cast<float> (dist_point (1)), static_cast<float> (dist_point (2))));
441  }
442  }
443  }
444 }
445 
446 #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>;
447 
448 #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_
Helper functor structure for copying data between an Eigen type and a PointT.
Definition: point_cloud.h:103
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.h:59
Helper functor structure for copying data between an Eigen type and a PointT.
Definition: point_cloud.h:70
Eigen::Vector3d mean_
3D voxel centroid
Eigen::Matrix3d icov_
Inverse of voxel covariance matrix.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
int getNeighborhoodAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors)
Get the voxels surrounding point p, not including the voxel contating point p.
const PointT & back() const
Definition: point_cloud.h:472
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:480
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
Simple structure to hold a centroid, covarince and the number of points in a leaf.
void getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud)
Get a cloud to visualize each voxels normal distribution.
A structure representing RGB color information.
Eigen::VectorXf centroid
Nd voxel centroid.
A point structure representing Euclidean xyz coordinates.
void applyFilter(PointCloud &output)
Filter cloud and initializes voxel structure.
Eigen::Matrix3d evecs_
Eigen vectors of voxel covariance matrix.
int nr_points
Number of points contained by voxel.
Eigen::Vector3d evals_
Eigen values of voxel covariance matrix.
Eigen::MatrixXi getAllNeighborCellIndices()
Get the relative cell indices of all the 26 neighbors.
Definition: voxel_grid.h:122
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:575
A point structure representing Euclidean xyz coordinates, and the RGB color.
Eigen::Matrix3d cov_
Voxel covariance matrix.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415