Point Cloud Library (PCL)  1.7.1
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 += 3;
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  // ---[ RGB special case
189  if (rgba_index >= 0)
190  {
191  // fill r/g/b data
192  int rgb;
193  memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
194  centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
195  centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
196  centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
197  }
198  pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
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  // ---[ RGB special case
249  if (rgba_index >= 0)
250  {
251  // Fill r/g/b data, assuming that the order is BGRA
252  int rgb;
253  memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
254  centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
255  centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
256  centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
257  }
258  pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
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  // pack r/g/b into rgb
317  float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], b = leaf.centroid[centroid_size - 1];
318  int rgb = (static_cast<int> (r)) << 16 | (static_cast<int> (g)) << 8 | (static_cast<int> (b));
319  memcpy (reinterpret_cast<char*> (&output.points.back ()) + rgba_index, &rgb, sizeof (float));
320  }
321  }
322 
323  // Stores the voxel indice for fast access searching
324  if (searchable_)
325  voxel_centroids_leaf_indices_.push_back (static_cast<int> (it->first));
326 
327  // Single pass covariance calculation
328  leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose ();
329  leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points;
330 
331  //Normalize Eigen Val such that max no more than 100x min.
332  eigensolver.compute (leaf.cov_);
333  eigen_val = eigensolver.eigenvalues ().asDiagonal ();
334  leaf.evecs_ = eigensolver.eigenvectors ();
335 
336  if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0)
337  {
338  leaf.nr_points = -1;
339  continue;
340  }
341 
342  // Avoids matrices near singularities (eq 6.11)[Magnusson 2009]
343 
344  min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2);
345  if (eigen_val (0, 0) < min_covar_eigvalue)
346  {
347  eigen_val (0, 0) = min_covar_eigvalue;
348 
349  if (eigen_val (1, 1) < min_covar_eigvalue)
350  {
351  eigen_val (1, 1) = min_covar_eigvalue;
352  }
353 
354  leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse ();
355  }
356  leaf.evals_ = eigen_val.diagonal ();
357 
358  leaf.icov_ = leaf.cov_.inverse ();
359  if (leaf.icov_.maxCoeff () == std::numeric_limits<float>::infinity ( )
360  || leaf.icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) )
361  {
362  leaf.nr_points = -1;
363  }
364 
365  }
366  }
367 
368  output.width = static_cast<uint32_t> (output.points.size ());
369 }
370 
371 //////////////////////////////////////////////////////////////////////////////////////////
372 template<typename PointT> int
373 pcl::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors)
374 {
375  neighbors.clear ();
376 
377  // Find displacement coordinates
378  Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices ();
379  Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x / leaf_size_[0])),
380  static_cast<int> (floor (reference_point.y / leaf_size_[1])),
381  static_cast<int> (floor (reference_point.z / leaf_size_[2])), 0);
382  Eigen::Array4i diff2min = min_b_ - ijk;
383  Eigen::Array4i diff2max = max_b_ - ijk;
384  neighbors.reserve (relative_coordinates.cols ());
385 
386  // Check each neighbor to see if it is occupied and contains sufficient points
387  // Slower than radius search because needs to check 26 indices
388  for (int ni = 0; ni < relative_coordinates.cols (); ni++)
389  {
390  Eigen::Vector4i displacement = (Eigen::Vector4i () << relative_coordinates.col (ni), 0).finished ();
391  // Checking if the specified cell is in the grid
392  if ((diff2min <= displacement.array ()).all () && (diff2max >= displacement.array ()).all ())
393  {
394  typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (((ijk + displacement - min_b_).dot (divb_mul_)));
395  if (leaf_iter != leaves_.end () && leaf_iter->second.nr_points >= min_points_per_voxel_)
396  {
397  LeafConstPtr leaf = &(leaf_iter->second);
398  neighbors.push_back (leaf);
399  }
400  }
401  }
402 
403  return (static_cast<int> (neighbors.size ()));
404 }
405 
406 //////////////////////////////////////////////////////////////////////////////////////////
407 template<typename PointT> void
409 {
410  cell_cloud.clear ();
411 
412  int pnt_per_cell = 1000;
413  boost::mt19937 rng;
414  boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
415  boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
416 
417  Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
418  Eigen::Matrix3d cholesky_decomp;
419  Eigen::Vector3d cell_mean;
420  Eigen::Vector3d rand_point;
421  Eigen::Vector3d dist_point;
422 
423  // Generate points for each occupied voxel with sufficient points.
424  for (typename std::map<size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
425  {
426  Leaf& leaf = it->second;
427 
428  if (leaf.nr_points >= min_points_per_voxel_)
429  {
430  cell_mean = leaf.mean_;
431  llt_of_cov.compute (leaf.cov_);
432  cholesky_decomp = llt_of_cov.matrixL ();
433 
434  // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix
435  for (int i = 0; i < pnt_per_cell; i++)
436  {
437  rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ());
438  dist_point = cell_mean + cholesky_decomp * rand_point;
439  cell_cloud.push_back (PointXYZ (static_cast<float> (dist_point (0)), static_cast<float> (dist_point (1)), static_cast<float> (dist_point (2))));
440  }
441  }
442  }
443 }
444 
445 #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>;
446 
447 #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_