Point Cloud Library (PCL)  1.7.2
mls.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-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 Willow Garage, Inc. 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  * $Id$
37  *
38  */
39 
40 #ifndef PCL_SURFACE_IMPL_MLS_H_
41 #define PCL_SURFACE_IMPL_MLS_H_
42 
43 #include <pcl/point_traits.h>
44 #include <pcl/surface/mls.h>
45 #include <pcl/common/io.h>
46 #include <pcl/common/copy_point.h>
47 #include <pcl/common/centroid.h>
48 #include <pcl/common/eigen.h>
49 #include <pcl/common/geometry.h>
50 
51 #ifdef _OPENMP
52 #include <omp.h>
53 #endif
54 
55 //////////////////////////////////////////////////////////////////////////////////////////////
56 template <typename PointInT, typename PointOutT> void
58 {
59  // Reset or initialize the collection of indices
60  corresponding_input_indices_.reset (new PointIndices);
61 
62  // Check if normals have to be computed/saved
63  if (compute_normals_)
64  {
65  normals_.reset (new NormalCloud);
66  // Copy the header
67  normals_->header = input_->header;
68  // Clear the fields in case the method exits before computation
69  normals_->width = normals_->height = 0;
70  normals_->points.clear ();
71  }
72 
73 
74  // Copy the header
75  output.header = input_->header;
76  output.width = output.height = 0;
77  output.points.clear ();
78 
79  if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
80  {
81  PCL_ERROR ("[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
82  return;
83  }
84 
85  // Check if distinct_cloud_ was set
86  if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
87  {
88  PCL_ERROR ("[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
89  return;
90  }
91 
92  if (!initCompute ())
93  return;
94 
95 
96  // Initialize the spatial locator
97  if (!tree_)
98  {
99  KdTreePtr tree;
100  if (input_->isOrganized ())
101  tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
102  else
103  tree.reset (new pcl::search::KdTree<PointInT> (false));
104  setSearchMethod (tree);
105  }
106 
107  // Send the surface dataset to the spatial locator
108  tree_->setInputCloud (input_);
109 
110  switch (upsample_method_)
111  {
112  // Initialize random number generator if necessary
113  case (RANDOM_UNIFORM_DENSITY):
114  {
115  rng_alg_.seed (static_cast<unsigned> (std::time (0)));
116  float tmp = static_cast<float> (search_radius_ / 2.0f);
117  boost::uniform_real<float> uniform_distrib (-tmp, tmp);
118  rng_uniform_distribution_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > (rng_alg_, uniform_distrib));
119 
120  mls_results_.resize (1); // Need to have a reference to a single dummy result.
121 
122  break;
123  }
124  case (VOXEL_GRID_DILATION):
125  case (DISTINCT_CLOUD):
126  {
127  mls_results_.resize (input_->size ());
128  break;
129  }
130  default:
131  {
132  mls_results_.resize (1); // Need to have a reference to a single dummy result.
133  break;
134  }
135  }
136 
137  // Perform the actual surface reconstruction
138  performProcessing (output);
139 
140  if (compute_normals_)
141  {
142  normals_->height = 1;
143  normals_->width = static_cast<uint32_t> (normals_->size ());
144 
145  for (unsigned int i = 0; i < output.size (); ++i)
146  {
147  typedef typename pcl::traits::fieldList<PointOutT>::type FieldList;
148  pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x));
149  pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y));
150  pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z));
151  pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature));
152  }
153 
154  }
155 
156  // Set proper widths and heights for the clouds
157  output.height = 1;
158  output.width = static_cast<uint32_t> (output.size ());
159 
160  deinitCompute ();
161 }
162 
163 //////////////////////////////////////////////////////////////////////////////////////////////
164 template <typename PointInT, typename PointOutT> void
166  const std::vector<int> &nn_indices,
167  std::vector<float> &nn_sqr_dists,
168  PointCloudOut &projected_points,
169  NormalCloud &projected_points_normals,
170  PointIndices &corresponding_input_indices,
171  MLSResult &mls_result) const
172 {
173  // Note: this method is const because it needs to be thread-safe
174  // (MovingLeastSquaresOMP calls it from multiple threads)
175 
176  // Compute the plane coefficients
177  EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
178  Eigen::Vector4d xyz_centroid;
179 
180  // Estimate the XYZ centroid
181  pcl::compute3DCentroid (*input_, nn_indices, xyz_centroid);
182 
183  // Compute the 3x3 covariance matrix
184  pcl::computeCovarianceMatrix (*input_, nn_indices, xyz_centroid, covariance_matrix);
185  EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
186  EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
187  Eigen::Vector4d model_coefficients;
188  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
189  model_coefficients.head<3> ().matrix () = eigen_vector;
190  model_coefficients[3] = 0;
191  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
192 
193  // Projected query point
194  Eigen::Vector3d point = input_->points[index].getVector3fMap ().template cast<double> ();
195  double distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
196  point -= distance * model_coefficients.head<3> ();
197 
198  float curvature = static_cast<float> (covariance_matrix.trace ());
199  // Compute the curvature surface change
200  if (curvature != 0)
201  curvature = fabsf (float (eigen_value / double (curvature)));
202 
203 
204  // Get a copy of the plane normal easy access
205  Eigen::Vector3d plane_normal = model_coefficients.head<3> ();
206  // Vector in which the polynomial coefficients will be put
207  Eigen::VectorXd c_vec;
208  // Local coordinate system (Darboux frame)
209  Eigen::Vector3d v_axis (0.0f, 0.0f, 0.0f), u_axis (0.0f, 0.0f, 0.0f);
210 
211 
212 
213  // Perform polynomial fit to update point and normal
214  ////////////////////////////////////////////////////
215  if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_)
216  {
217  // Update neighborhood, since point was projected, and computing relative
218  // positions. Note updating only distances for the weights for speed
219  std::vector<Eigen::Vector3d> de_meaned (nn_indices.size ());
220  for (size_t ni = 0; ni < nn_indices.size (); ++ni)
221  {
222  de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
223  de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1];
224  de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2];
225  nn_sqr_dists[ni] = static_cast<float> (de_meaned[ni].dot (de_meaned[ni]));
226  }
227 
228  // Allocate matrices and vectors to hold the data used for the polynomial fit
229  Eigen::VectorXd weight_vec (nn_indices.size ());
230  Eigen::MatrixXd P (nr_coeff_, nn_indices.size ());
231  Eigen::VectorXd f_vec (nn_indices.size ());
232  Eigen::MatrixXd P_weight; // size will be (nr_coeff_, nn_indices.size ());
233  Eigen::MatrixXd P_weight_Pt (nr_coeff_, nr_coeff_);
234 
235  // Get local coordinate system (Darboux frame)
236  v_axis = plane_normal.unitOrthogonal ();
237  u_axis = plane_normal.cross (v_axis);
238 
239  // Go through neighbors, transform them in the local coordinate system,
240  // save height and the evaluation of the polynome's terms
241  double u_coord, v_coord, u_pow, v_pow;
242  for (size_t ni = 0; ni < nn_indices.size (); ++ni)
243  {
244  // (Re-)compute weights
245  weight_vec (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_);
246 
247  // Transforming coordinates
248  u_coord = de_meaned[ni].dot (u_axis);
249  v_coord = de_meaned[ni].dot (v_axis);
250  f_vec (ni) = de_meaned[ni].dot (plane_normal);
251 
252  // Compute the polynomial's terms at the current point
253  int j = 0;
254  u_pow = 1;
255  for (int ui = 0; ui <= order_; ++ui)
256  {
257  v_pow = 1;
258  for (int vi = 0; vi <= order_ - ui; ++vi)
259  {
260  P (j++, ni) = u_pow * v_pow;
261  v_pow *= v_coord;
262  }
263  u_pow *= u_coord;
264  }
265  }
266 
267  // Computing coefficients
268  P_weight = P * weight_vec.asDiagonal ();
269  P_weight_Pt = P_weight * P.transpose ();
270  c_vec = P_weight * f_vec;
271  P_weight_Pt.llt ().solveInPlace (c_vec);
272  }
273 
274  switch (upsample_method_)
275  {
276  case (NONE):
277  {
278  Eigen::Vector3d normal = plane_normal;
279 
280  if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
281  {
282  point += (c_vec[0] * plane_normal);
283 
284  // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
285  if (compute_normals_)
286  normal = plane_normal - c_vec[order_ + 1] * u_axis - c_vec[1] * v_axis;
287  }
288 
289  PointOutT aux;
290  aux.x = static_cast<float> (point[0]);
291  aux.y = static_cast<float> (point[1]);
292  aux.z = static_cast<float> (point[2]);
293  projected_points.push_back (aux);
294 
295  if (compute_normals_)
296  {
297  pcl::Normal aux_normal;
298  aux_normal.normal_x = static_cast<float> (normal[0]);
299  aux_normal.normal_y = static_cast<float> (normal[1]);
300  aux_normal.normal_z = static_cast<float> (normal[2]);
301  aux_normal.curvature = curvature;
302  projected_points_normals.push_back (aux_normal);
303  corresponding_input_indices.indices.push_back (index);
304  }
305 
306  break;
307  }
308 
309  case (SAMPLE_LOCAL_PLANE):
310  {
311  // Uniformly sample a circle around the query point using the radius and step parameters
312  for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
313  for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
314  if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_)
315  {
316  PointOutT projected_point;
317  pcl::Normal projected_normal;
318  projectPointToMLSSurface (u_disp, v_disp, u_axis, v_axis, plane_normal, point,
319  curvature, c_vec,
320  static_cast<int> (nn_indices.size ()),
321  projected_point, projected_normal);
322 
323  projected_points.push_back (projected_point);
324  corresponding_input_indices.indices.push_back (index);
325  if (compute_normals_)
326  projected_points_normals.push_back (projected_normal);
327  }
328  break;
329  }
330 
331  case (RANDOM_UNIFORM_DENSITY):
332  {
333  // Compute the local point density and add more samples if necessary
334  int num_points_to_add = static_cast<int> (floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
335 
336  // Just add the query point, because the density is good
337  if (num_points_to_add <= 0)
338  {
339  // Just add the current point
340  Eigen::Vector3d normal = plane_normal;
341  if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
342  {
343  // Projection onto MLS surface along Darboux normal to the height at (0,0)
344  point += (c_vec[0] * plane_normal);
345  // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
346  if (compute_normals_)
347  normal = plane_normal - c_vec[order_ + 1] * u_axis - c_vec[1] * v_axis;
348  }
349  PointOutT aux;
350  aux.x = static_cast<float> (point[0]);
351  aux.y = static_cast<float> (point[1]);
352  aux.z = static_cast<float> (point[2]);
353  projected_points.push_back (aux);
354  corresponding_input_indices.indices.push_back (index);
355 
356  if (compute_normals_)
357  {
358  pcl::Normal aux_normal;
359  aux_normal.normal_x = static_cast<float> (normal[0]);
360  aux_normal.normal_y = static_cast<float> (normal[1]);
361  aux_normal.normal_z = static_cast<float> (normal[2]);
362  aux_normal.curvature = curvature;
363  projected_points_normals.push_back (aux_normal);
364  }
365  }
366  else
367  {
368  // Sample the local plane
369  for (int num_added = 0; num_added < num_points_to_add;)
370  {
371  float u_disp = (*rng_uniform_distribution_) (),
372  v_disp = (*rng_uniform_distribution_) ();
373  // Check if inside circle; if not, try another coin flip
374  if (u_disp * u_disp + v_disp * v_disp > search_radius_ * search_radius_/4)
375  continue;
376 
377 
378  PointOutT projected_point;
379  pcl::Normal projected_normal;
380  projectPointToMLSSurface (u_disp, v_disp, u_axis, v_axis, plane_normal, point,
381  curvature, c_vec,
382  static_cast<int> (nn_indices.size ()),
383  projected_point, projected_normal);
384 
385  projected_points.push_back (projected_point);
386  corresponding_input_indices.indices.push_back (index);
387  if (compute_normals_)
388  projected_points_normals.push_back (projected_normal);
389 
390  num_added ++;
391  }
392  }
393  break;
394  }
395 
396  case (VOXEL_GRID_DILATION):
397  case (DISTINCT_CLOUD):
398  {
399  // Take all point pairs and sample space between them in a grid-fashion
400  // \note consider only point pairs with increasing indices
401  mls_result = MLSResult (point, plane_normal, u_axis, v_axis, c_vec, static_cast<int> (nn_indices.size ()), curvature);
402  break;
403  }
404  }
405 }
406 
407 //////////////////////////////////////////////////////////////////////////////////////////////
408 template <typename PointInT, typename PointOutT> void
410  Eigen::Vector3d &u, Eigen::Vector3d &v,
411  Eigen::Vector3d &plane_normal,
412  Eigen::Vector3d &mean,
413  float &curvature,
414  Eigen::VectorXd &c_vec,
415  int num_neighbors,
416  PointOutT &result_point,
417  pcl::Normal &result_normal) const
418 {
419  double n_disp = 0.0f;
420  double d_u = 0.0f, d_v = 0.0f;
421 
422  // HARDCODED 5*nr_coeff_ to guarantee that the computed polynomial had a proper point set basis
423  if (polynomial_fit_ && num_neighbors >= 5*nr_coeff_ && pcl_isfinite (c_vec[0]))
424  {
425  // Compute the displacement along the normal using the fitted polynomial
426  // and compute the partial derivatives needed for estimating the normal
427  int j = 0;
428  float u_pow = 1.0f, v_pow = 1.0f, u_pow_prev = 1.0f, v_pow_prev = 1.0f;
429  for (int ui = 0; ui <= order_; ++ui)
430  {
431  v_pow = 1;
432  for (int vi = 0; vi <= order_ - ui; ++vi)
433  {
434  // Compute displacement along normal
435  n_disp += u_pow * v_pow * c_vec[j++];
436 
437  // Compute partial derivatives
438  if (ui >= 1)
439  d_u += c_vec[j-1] * ui * u_pow_prev * v_pow;
440  if (vi >= 1)
441  d_v += c_vec[j-1] * vi * u_pow * v_pow_prev;
442 
443  v_pow_prev = v_pow;
444  v_pow *= v_disp;
445  }
446  u_pow_prev = u_pow;
447  u_pow *= u_disp;
448  }
449  }
450 
451  result_point.x = static_cast<float> (mean[0] + u[0] * u_disp + v[0] * v_disp + plane_normal[0] * n_disp);
452  result_point.y = static_cast<float> (mean[1] + u[1] * u_disp + v[1] * v_disp + plane_normal[1] * n_disp);
453  result_point.z = static_cast<float> (mean[2] + u[2] * u_disp + v[2] * v_disp + plane_normal[2] * n_disp);
454 
455  Eigen::Vector3d normal = plane_normal - d_u * u - d_v * v;
456  normal.normalize ();
457 
458  result_normal.normal_x = static_cast<float> (normal[0]);
459  result_normal.normal_y = static_cast<float> (normal[1]);
460  result_normal.normal_z = static_cast<float> (normal[2]);
461  result_normal.curvature = curvature;
462 }
463 
464 //////////////////////////////////////////////////////////////////////////////////////////////
465 template <typename PointInT, typename PointOutT> void
467 {
468  // Compute the number of coefficients
469  nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
470 
471  // Allocate enough space to hold the results of nearest neighbor searches
472  // \note resize is irrelevant for a radiusSearch ().
473  std::vector<int> nn_indices;
474  std::vector<float> nn_sqr_dists;
475 
476  size_t mls_result_index = 0;
477 
478  // For all points
479  for (size_t cp = 0; cp < indices_->size (); ++cp)
480  {
481  // Get the initial estimates of point positions and their neighborhoods
482  if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
483  continue;
484 
485 
486  // Check the number of nearest neighbors for normal estimation (and later
487  // for polynomial fit as well)
488  if (nn_indices.size () < 3)
489  continue;
490 
491 
492  PointCloudOut projected_points;
493  NormalCloud projected_points_normals;
494  // Get a plane approximating the local surface's tangent and project point onto it
495  int index = (*indices_)[cp];
496 
497  if (upsample_method_ == VOXEL_GRID_DILATION || upsample_method_ == DISTINCT_CLOUD)
498  mls_result_index = index; // otherwise we give it a dummy location.
499 
500  computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
501 
502 
503  // Copy all information from the input cloud to the output points (not doing any interpolation)
504  for (size_t pp = 0; pp < projected_points.size (); ++pp)
505  copyMissingFields (input_->points[(*indices_)[cp]], projected_points[pp]);
506 
507 
508  // Append projected points to output
509  output.insert (output.end (), projected_points.begin (), projected_points.end ());
510  if (compute_normals_)
511  normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
512  }
513 
514  // Perform the distinct-cloud or voxel-grid upsampling
515  performUpsampling (output);
516 }
517 
518 //////////////////////////////////////////////////////////////////////////////////////////////
519 #ifdef _OPENMP
520 template <typename PointInT, typename PointOutT> void
521 pcl::MovingLeastSquaresOMP<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
522 {
523  // Compute the number of coefficients
524  nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
525 
526  // (Maximum) number of threads
527  unsigned int threads = threads_ == 0 ? 1 : threads_;
528 
529  // Create temporaries for each thread in order to avoid synchronization
530  typename PointCloudOut::CloudVectorType projected_points (threads);
531  typename NormalCloud::CloudVectorType projected_points_normals (threads);
532  std::vector<PointIndices> corresponding_input_indices (threads);
533 
534  // For all points
535 #pragma omp parallel for schedule (dynamic,1000) num_threads (threads)
536  for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
537  {
538  // Allocate enough space to hold the results of nearest neighbor searches
539  // \note resize is irrelevant for a radiusSearch ().
540  std::vector<int> nn_indices;
541  std::vector<float> nn_sqr_dists;
542 
543  // Get the initial estimates of point positions and their neighborhoods
544  if (this->searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
545  {
546  // Check the number of nearest neighbors for normal estimation (and later
547  // for polynomial fit as well)
548  if (nn_indices.size () >= 3)
549  {
550  // This thread's ID (range 0 to threads-1)
551  int tn = omp_get_thread_num ();
552 
553  // Size of projected points before computeMLSPointNormal () adds points
554  size_t pp_size = projected_points[tn].size ();
555 
556  // Get a plane approximating the local surface's tangent and project point onto it
557  int index = (*indices_)[cp];
558  size_t mls_result_index = 0;
559 
560  if (upsample_method_ == VOXEL_GRID_DILATION || upsample_method_ == DISTINCT_CLOUD)
561  mls_result_index = index; // otherwise we give it a dummy location.
562 
563  this->computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[mls_result_index]);
564 
565  // Copy all information from the input cloud to the output points (not doing any interpolation)
566  for (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp)
567  this->copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]);
568  }
569  }
570  }
571 
572 
573  // Combine all threads' results into the output vectors
574  for (unsigned int tn = 0; tn < threads; ++tn)
575  {
576  output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ());
577  corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
578  corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
579  if (compute_normals_)
580  normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
581  }
582 
583  // Perform the distinct-cloud or voxel-grid upsampling
584  this->performUpsampling (output);
585 }
586 #endif
587 
588 //////////////////////////////////////////////////////////////////////////////////////////////
589 template <typename PointInT, typename PointOutT> void
591 {
592  if (upsample_method_ == DISTINCT_CLOUD)
593  {
594  for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i
595  {
596  // Distinct cloud may have nan points, skip them
597  if (!pcl_isfinite (distinct_cloud_->points[dp_i].x))
598  continue;
599 
600  // Get 3D position of point
601  //Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap ();
602  std::vector<int> nn_indices;
603  std::vector<float> nn_dists;
604  tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists);
605  int input_index = nn_indices.front ();
606 
607  // If the closest point did not have a valid MLS fitting result
608  // OR if it is too far away from the sampled point
609  if (mls_results_[input_index].valid == false)
610  continue;
611 
612  Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast<double> ();
613 
614  float u_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)),
615  v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis));
616 
617  PointOutT result_point;
618  pcl::Normal result_normal;
619  projectPointToMLSSurface (u_disp, v_disp,
620  mls_results_[input_index].u_axis, mls_results_[input_index].v_axis,
621  mls_results_[input_index].plane_normal,
622  mls_results_[input_index].mean,
623  mls_results_[input_index].curvature,
624  mls_results_[input_index].c_vec,
625  mls_results_[input_index].num_neighbors,
626  result_point, result_normal);
627 
628  // Copy additional point information if available
629  copyMissingFields (input_->points[input_index], result_point);
630 
631  // Store the id of the original point
632  corresponding_input_indices_->indices.push_back (input_index);
633 
634  output.push_back (result_point);
635  if (compute_normals_)
636  normals_->push_back (result_normal);
637  }
638  }
639 
640  // For the voxel grid upsampling method, generate the voxel grid and dilate it
641  // Then, project the newly obtained points to the MLS surface
642  if (upsample_method_ == VOXEL_GRID_DILATION)
643  {
644  MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
645  for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
646  voxel_grid.dilate ();
647 
648  for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it)
649  {
650  // Get 3D position of point
651  Eigen::Vector3f pos;
652  voxel_grid.getPosition (m_it->first, pos);
653 
654  PointInT p;
655  p.x = pos[0];
656  p.y = pos[1];
657  p.z = pos[2];
658 
659  std::vector<int> nn_indices;
660  std::vector<float> nn_dists;
661  tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
662  int input_index = nn_indices.front ();
663 
664  // If the closest point did not have a valid MLS fitting result
665  // OR if it is too far away from the sampled point
666  if (mls_results_[input_index].valid == false)
667  continue;
668 
669  Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
670  float u_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)),
671  v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis));
672 
673  PointOutT result_point;
674  pcl::Normal result_normal;
675  projectPointToMLSSurface (u_disp, v_disp,
676  mls_results_[input_index].u_axis, mls_results_[input_index].v_axis,
677  mls_results_[input_index].plane_normal,
678  mls_results_[input_index].mean,
679  mls_results_[input_index].curvature,
680  mls_results_[input_index].c_vec,
681  mls_results_[input_index].num_neighbors,
682  result_point, result_normal);
683 
684  // Copy additional point information if available
685  copyMissingFields (input_->points[input_index], result_point);
686 
687  // Store the id of the original point
688  corresponding_input_indices_->indices.push_back (input_index);
689 
690  output.push_back (result_point);
691 
692  if (compute_normals_)
693  normals_->push_back (result_normal);
694  }
695  }
696 }
697 
698 //////////////////////////////////////////////////////////////////////////////////////////////
699 template <typename PointInT, typename PointOutT>
701  const Eigen::Vector3d &a_plane_normal,
702  const Eigen::Vector3d &a_u,
703  const Eigen::Vector3d &a_v,
704  const Eigen::VectorXd a_c_vec,
705  const int a_num_neighbors,
706  const float &a_curvature) :
707  mean (a_mean), plane_normal (a_plane_normal), u_axis (a_u), v_axis (a_v), c_vec (a_c_vec), num_neighbors (a_num_neighbors),
708  curvature (a_curvature), valid (true)
709 {
710 }
711 
712 //////////////////////////////////////////////////////////////////////////////////////////////
713 template <typename PointInT, typename PointOutT>
715  IndicesPtr &indices,
716  float voxel_size) :
717  voxel_grid_ (), bounding_min_ (), bounding_max_ (), data_size_ (), voxel_size_ (voxel_size)
718 {
719  pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);
720 
721  Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;
722  double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
723  // Put initial cloud in voxel grid
724  data_size_ = static_cast<uint64_t> (1.5 * max_size / voxel_size_);
725  for (unsigned int i = 0; i < indices->size (); ++i)
726  if (pcl_isfinite (cloud->points[(*indices)[i]].x))
727  {
728  Eigen::Vector3i pos;
729  getCellIndex (cloud->points[(*indices)[i]].getVector3fMap (), pos);
730 
731  uint64_t index_1d;
732  getIndexIn1D (pos, index_1d);
733  Leaf leaf;
734  voxel_grid_[index_1d] = leaf;
735  }
736 }
737 
738 //////////////////////////////////////////////////////////////////////////////////////////////
739 template <typename PointInT, typename PointOutT> void
741 {
742  HashMap new_voxel_grid = voxel_grid_;
743  for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
744  {
745  Eigen::Vector3i index;
746  getIndexIn3D (m_it->first, index);
747 
748  // Now dilate all of its voxels
749  for (int x = -1; x <= 1; ++x)
750  for (int y = -1; y <= 1; ++y)
751  for (int z = -1; z <= 1; ++z)
752  if (x != 0 || y != 0 || z != 0)
753  {
754  Eigen::Vector3i new_index;
755  new_index = index + Eigen::Vector3i (x, y, z);
756 
757  uint64_t index_1d;
758  getIndexIn1D (new_index, index_1d);
759  Leaf leaf;
760  new_voxel_grid[index_1d] = leaf;
761  }
762  }
763  voxel_grid_ = new_voxel_grid;
764 }
765 
766 
767 /////////////////////////////////////////////////////////////////////////////////////////////
768 template <typename PointInT, typename PointOutT> void
770  PointOutT &point_out) const
771 {
772  PointOutT temp = point_out;
773  copyPoint (point_in, point_out);
774  point_out.x = temp.x;
775  point_out.y = temp.y;
776  point_out.z = temp.z;
777 }
778 
779 
780 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;
781 
782 #ifdef _OPENMP
783 #define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>;
784 #endif
785 
786 #endif // PCL_SURFACE_IMPL_MLS_H_
A point structure representing normal coordinates and the surface curvature estimate.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
A helper functor that can set a specific value in a field if the field exists.
Definition: point_traits.h:291
struct pcl::_PointXYZHSV EIGEN_ALIGN16
virtual void performProcessing(PointCloudOut &output)
Abstract surface reconstruction method.
Definition: mls.hpp:466
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
iterator begin()
Definition: point_cloud.h:434
Eigen::Vector4f bounding_max_
Definition: mls.h:423
std::vector< int > indices
Definition: PointIndices.h:19
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
void projectPointToMLSSurface(float &u_disp, float &v_disp, Eigen::Vector3d &u_axis, Eigen::Vector3d &v_axis, Eigen::Vector3d &n_axis, Eigen::Vector3d &mean, float &curvature, Eigen::VectorXd &c_vec, int num_neighbors, PointOutT &result_point, pcl::Normal &result_normal) const
Fits a point (sample point) given in the local plane coordinates of an input point (query point) to t...
Definition: mls.hpp:409
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:472
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
Eigen::Vector4f bounding_min_
Definition: mls.h:423
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
Definition: point_cloud.h:486
void computeMLSPointNormal(int index, const std::vector< int > &nn_indices, std::vector< float > &nn_sqr_dists, PointCloudOut &projected_points, NormalCloud &projected_points_normals, PointIndices &corresponding_input_indices, MLSResult &mls_result) const
Smooth a given point and its neighborghood using Moving Least Squares.
Definition: mls.hpp:165
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
pcl::search::Search< PointInT >::Ptr KdTreePtr
Definition: mls.h:78
void performUpsampling(PointCloudOut &output)
Perform upsampling for the distinct-cloud and voxel-grid methods.
Definition: mls.hpp:590
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
Definition: common.hpp:212
size_t size() const
Definition: point_cloud.h:440
void process(PointCloudOut &output)
Base method for surface reconstruction for all points given in &lt;setInputCloud (), setIndices ()&gt; ...
Definition: mls.hpp:57
PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: mls.h:88
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:251
iterator end()
Definition: point_cloud.h:435
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
Definition: mls.h:376
MLSVoxelGrid(PointCloudInConstPtr &cloud, IndicesPtr &indices, float voxel_size)
Definition: mls.hpp:714
void getIndexIn1D(const Eigen::Vector3i &index, uint64_t &index_1d) const
Definition: mls.h:389
float voxel_size_
Voxel size for the VOXEL_GRID_DILATION upsampling method.
Definition: mls.h:430
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
Definition: organized.h:62
void getPosition(const uint64_t &index_1d, Eigen::Vector3f &point) const
Definition: mls.h:413
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition: centroid.hpp:50
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
Definition: mls.h:406
void copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
Definition: mls.hpp:769
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:138
Data structure used to store the results of the MLS fitting.
Definition: mls.h:348
std::map< uint64_t, Leaf > HashMap
Definition: mls.h:421
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415