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