Point Cloud Library (PCL)  1.12.1-dev
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/type_traits.h>
44 #include <pcl/surface/mls.h>
45 #include <pcl/common/common.h> // for getMinMax3D
46 #include <pcl/common/copy_point.h>
47 #include <pcl/common/centroid.h>
48 #include <pcl/common/eigen.h>
49 #include <pcl/search/kdtree.h> // for KdTree
50 #include <pcl/search/organized.h> // for OrganizedNeighbor
51 
52 #include <Eigen/Geometry> // for cross
53 #include <Eigen/LU> // for inverse
54 
55 #ifdef _OPENMP
56 #include <omp.h>
57 #endif
58 
59 //////////////////////////////////////////////////////////////////////////////////////////////
60 template <typename PointInT, typename PointOutT> void
62 {
63  // Reset or initialize the collection of indices
64  corresponding_input_indices_.reset (new PointIndices);
65 
66  // Check if normals have to be computed/saved
67  if (compute_normals_)
68  {
69  normals_.reset (new NormalCloud);
70  // Copy the header
71  normals_->header = input_->header;
72  // Clear the fields in case the method exits before computation
73  normals_->width = normals_->height = 0;
74  normals_->points.clear ();
75  }
76 
77  // Copy the header
78  output.header = input_->header;
79  output.width = output.height = 0;
80  output.clear ();
81 
82  if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
83  {
84  PCL_ERROR ("[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
85  return;
86  }
87 
88  // Check if distinct_cloud_ was set
89  if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
90  {
91  PCL_ERROR ("[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
92  return;
93  }
94 
95  if (!initCompute ())
96  return;
97 
98  // Initialize the spatial locator
99  if (!tree_)
100  {
101  KdTreePtr tree;
102  if (input_->isOrganized ())
103  tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
104  else
105  tree.reset (new pcl::search::KdTree<PointInT> (false));
106  setSearchMethod (tree);
107  }
108 
109  // Send the surface dataset to the spatial locator
110  tree_->setInputCloud (input_);
111 
112  switch (upsample_method_)
113  {
114  // Initialize random number generator if necessary
115  case (RANDOM_UNIFORM_DENSITY):
116  {
117  std::random_device rd;
118  rng_.seed (rd());
119  const double tmp = search_radius_ / 2.0;
120  rng_uniform_distribution_.reset (new std::uniform_real_distribution<> (-tmp, tmp));
121 
122  break;
123  }
124  case (VOXEL_GRID_DILATION):
125  case (DISTINCT_CLOUD):
126  {
127  if (!cache_mls_results_)
128  PCL_WARN ("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");
129 
130  cache_mls_results_ = true;
131  break;
132  }
133  default:
134  break;
135  }
136 
137  if (cache_mls_results_)
138  {
139  mls_results_.resize (input_->size ());
140  }
141  else
142  {
143  mls_results_.resize (1); // Need to have a reference to a single dummy result.
144  }
145 
146  // Perform the actual surface reconstruction
147  performProcessing (output);
148 
149  if (compute_normals_)
150  {
151  normals_->height = 1;
152  normals_->width = normals_->size ();
153 
154  for (std::size_t i = 0; i < output.size (); ++i)
155  {
156  using FieldList = typename pcl::traits::fieldList<PointOutT>::type;
157  pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "normal_x", (*normals_)[i].normal_x));
158  pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "normal_y", (*normals_)[i].normal_y));
159  pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "normal_z", (*normals_)[i].normal_z));
160  pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output[i], "curvature", (*normals_)[i].curvature));
161  }
162 
163  }
164 
165  // Set proper widths and heights for the clouds
166  output.height = 1;
167  output.width = output.size ();
168 
169  deinitCompute ();
170 }
171 
172 //////////////////////////////////////////////////////////////////////////////////////////////
173 template <typename PointInT, typename PointOutT> void
175  const pcl::Indices &nn_indices,
176  PointCloudOut &projected_points,
177  NormalCloud &projected_points_normals,
178  PointIndices &corresponding_input_indices,
179  MLSResult &mls_result) const
180 {
181  // Note: this method is const because it needs to be thread-safe
182  // (MovingLeastSquaresOMP calls it from multiple threads)
183 
184  mls_result.computeMLSSurface<PointInT> (*input_, index, nn_indices, search_radius_, order_);
185 
186  switch (upsample_method_)
187  {
188  case (NONE):
189  {
190  const MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint (projection_method_, nr_coeff_);
191  addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
192  break;
193  }
194 
195  case (SAMPLE_LOCAL_PLANE):
196  {
197  // Uniformly sample a circle around the query point using the radius and step parameters
198  for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
199  for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
200  if (u_disp * u_disp + v_disp * v_disp < upsampling_radius_ * upsampling_radius_)
201  {
203  addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
204  }
205  break;
206  }
207 
208  case (RANDOM_UNIFORM_DENSITY):
209  {
210  // Compute the local point density and add more samples if necessary
211  const int num_points_to_add = static_cast<int> (std::floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
212 
213  // Just add the query point, because the density is good
214  if (num_points_to_add <= 0)
215  {
216  // Just add the current point
217  const MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint (projection_method_, nr_coeff_);
218  addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
219  }
220  else
221  {
222  // Sample the local plane
223  for (int num_added = 0; num_added < num_points_to_add;)
224  {
225  const double u = (*rng_uniform_distribution_) (rng_);
226  const double v = (*rng_uniform_distribution_) (rng_);
227 
228  // Check if inside circle; if not, try another coin flip
229  if (u * u + v * v > search_radius_ * search_radius_ / 4)
230  continue;
231 
233  if (order_ > 1 && mls_result.num_neighbors >= 5 * nr_coeff_)
234  proj = mls_result.projectPointSimpleToPolynomialSurface (u, v);
235  else
236  proj = mls_result.projectPointToMLSPlane (u, v);
237 
238  addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
239 
240  num_added++;
241  }
242  }
243  break;
244  }
245 
246  default:
247  break;
248  }
249 }
250 
251 template <typename PointInT, typename PointOutT> void
253  const Eigen::Vector3d &point,
254  const Eigen::Vector3d &normal,
255  double curvature,
256  PointCloudOut &projected_points,
257  NormalCloud &projected_points_normals,
258  PointIndices &corresponding_input_indices) const
259 {
260  PointOutT aux;
261  aux.x = static_cast<float> (point[0]);
262  aux.y = static_cast<float> (point[1]);
263  aux.z = static_cast<float> (point[2]);
264 
265  // Copy additional point information if available
266  copyMissingFields ((*input_)[index], aux);
267 
268  projected_points.push_back (aux);
269  corresponding_input_indices.indices.push_back (index);
270 
271  if (compute_normals_)
272  {
273  pcl::Normal aux_normal;
274  aux_normal.normal_x = static_cast<float> (normal[0]);
275  aux_normal.normal_y = static_cast<float> (normal[1]);
276  aux_normal.normal_z = static_cast<float> (normal[2]);
277  aux_normal.curvature = curvature;
278  projected_points_normals.push_back (aux_normal);
279  }
280 }
281 
282 //////////////////////////////////////////////////////////////////////////////////////////////
283 template <typename PointInT, typename PointOutT> void
285 {
286  // Compute the number of coefficients
287  nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;
288 
289 #ifdef _OPENMP
290  // (Maximum) number of threads
291  const unsigned int threads = threads_ == 0 ? 1 : threads_;
292  // Create temporaries for each thread in order to avoid synchronization
293  typename PointCloudOut::CloudVectorType projected_points (threads);
294  typename NormalCloud::CloudVectorType projected_points_normals (threads);
295  std::vector<PointIndices> corresponding_input_indices (threads);
296 #endif
297 
298  // For all points
299 #pragma omp parallel for \
300  default(none) \
301  shared(corresponding_input_indices, projected_points, projected_points_normals) \
302  schedule(dynamic,1000) \
303  num_threads(threads)
304  for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
305  {
306  // Allocate enough space to hold the results of nearest neighbor searches
307  // \note resize is irrelevant for a radiusSearch ().
308  pcl::Indices nn_indices;
309  std::vector<float> nn_sqr_dists;
310 
311  // Get the initial estimates of point positions and their neighborhoods
312  if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
313  {
314  // Check the number of nearest neighbors for normal estimation (and later for polynomial fit as well)
315  if (nn_indices.size () >= 3)
316  {
317  // This thread's ID (range 0 to threads-1)
318 #ifdef _OPENMP
319  const int tn = omp_get_thread_num ();
320  // Size of projected points before computeMLSPointNormal () adds points
321  std::size_t pp_size = projected_points[tn].size ();
322 #else
323  PointCloudOut projected_points;
324  NormalCloud projected_points_normals;
325 #endif
326 
327  // Get a plane approximating the local surface's tangent and project point onto it
328  const int index = (*indices_)[cp];
329 
330  std::size_t mls_result_index = 0;
331  if (cache_mls_results_)
332  mls_result_index = index; // otherwise we give it a dummy location.
333 
334 #ifdef _OPENMP
335  computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]);
336 
337  // Copy all information from the input cloud to the output points (not doing any interpolation)
338  for (std::size_t pp = pp_size; pp < projected_points[tn].size (); ++pp)
339  copyMissingFields ((*input_)[(*indices_)[cp]], projected_points[tn][pp]);
340 #else
341  computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
342 
343  // Append projected points to output
344  output.insert (output.end (), projected_points.begin (), projected_points.end ());
345  if (compute_normals_)
346  normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
347 #endif
348  }
349  }
350  }
351 
352 #ifdef _OPENMP
353  // Combine all threads' results into the output vectors
354  for (unsigned int tn = 0; tn < threads; ++tn)
355  {
356  output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ());
357  corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
358  corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
359  if (compute_normals_)
360  normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
361  }
362 #endif
363 
364  // Perform the distinct-cloud or voxel-grid upsampling
365  performUpsampling (output);
366 }
367 
368 //////////////////////////////////////////////////////////////////////////////////////////////
369 template <typename PointInT, typename PointOutT> void
371 {
372 
373  if (upsample_method_ == DISTINCT_CLOUD)
374  {
375  corresponding_input_indices_.reset (new PointIndices);
376  for (std::size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i
377  {
378  // Distinct cloud may have nan points, skip them
379  if (!std::isfinite ((*distinct_cloud_)[dp_i].x))
380  continue;
381 
382  // Get 3D position of point
383  //Eigen::Vector3f pos = (*distinct_cloud_)[dp_i].getVector3fMap ();
384  pcl::Indices nn_indices;
385  std::vector<float> nn_dists;
386  tree_->nearestKSearch ((*distinct_cloud_)[dp_i], 1, nn_indices, nn_dists);
387  const auto input_index = nn_indices.front ();
388 
389  // If the closest point did not have a valid MLS fitting result
390  // OR if it is too far away from the sampled point
391  if (mls_results_[input_index].valid == false)
392  continue;
393 
394  Eigen::Vector3d add_point = (*distinct_cloud_)[dp_i].getVector3fMap ().template cast<double> ();
395  MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_);
396  addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
397  }
398  }
399 
400  // For the voxel grid upsampling method, generate the voxel grid and dilate it
401  // Then, project the newly obtained points to the MLS surface
402  if (upsample_method_ == VOXEL_GRID_DILATION)
403  {
404  corresponding_input_indices_.reset (new PointIndices);
405 
406  MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_, dilation_iteration_num_);
407  for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
408  voxel_grid.dilate ();
409 
410  for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it)
411  {
412  // Get 3D position of point
413  Eigen::Vector3f pos;
414  voxel_grid.getPosition (m_it->first, pos);
415 
416  PointInT p;
417  p.x = pos[0];
418  p.y = pos[1];
419  p.z = pos[2];
420 
421  pcl::Indices nn_indices;
422  std::vector<float> nn_dists;
423  tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
424  const auto input_index = nn_indices.front ();
425 
426  // If the closest point did not have a valid MLS fitting result
427  // OR if it is too far away from the sampled point
428  if (mls_results_[input_index].valid == false)
429  continue;
430 
431  Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
432  MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_);
433  addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
434  }
435  }
436 }
437 
438 //////////////////////////////////////////////////////////////////////////////////////////////
439 pcl::MLSResult::MLSResult (const Eigen::Vector3d &a_query_point,
440  const Eigen::Vector3d &a_mean,
441  const Eigen::Vector3d &a_plane_normal,
442  const Eigen::Vector3d &a_u,
443  const Eigen::Vector3d &a_v,
444  const Eigen::VectorXd &a_c_vec,
445  const int a_num_neighbors,
446  const float a_curvature,
447  const int a_order) :
448  query_point (a_query_point), 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),
449  curvature (a_curvature), order (a_order), valid (true)
450 {}
451 
452 void
453 pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v, double &w) const
454 {
455  Eigen::Vector3d delta = pt - mean;
456  u = delta.dot (u_axis);
457  v = delta.dot (v_axis);
458  w = delta.dot (plane_normal);
459 }
460 
461 void
462 pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v) const
463 {
464  Eigen::Vector3d delta = pt - mean;
465  u = delta.dot (u_axis);
466  v = delta.dot (v_axis);
467 }
468 
469 double
470 pcl::MLSResult::getPolynomialValue (const double u, const double v) const
471 {
472  // Compute the polynomial's terms at the current point
473  // Example for second order: z = a + b*y + c*y^2 + d*x + e*x*y + f*x^2
474  int j = 0;
475  double u_pow = 1;
476  double result = 0;
477  for (int ui = 0; ui <= order; ++ui)
478  {
479  double v_pow = 1;
480  for (int vi = 0; vi <= order - ui; ++vi)
481  {
482  result += c_vec[j++] * u_pow * v_pow;
483  v_pow *= v;
484  }
485  u_pow *= u;
486  }
487 
488  return (result);
489 }
490 
492 pcl::MLSResult::getPolynomialPartialDerivative (const double u, const double v) const
493 {
494  // Compute the displacement along the normal using the fitted polynomial
495  // and compute the partial derivatives needed for estimating the normal
497  Eigen::VectorXd u_pow (order + 2), v_pow (order + 2);
498  int j = 0;
499 
500  d.z = d.z_u = d.z_v = d.z_uu = d.z_vv = d.z_uv = 0;
501  u_pow (0) = v_pow (0) = 1;
502  for (int ui = 0; ui <= order; ++ui)
503  {
504  for (int vi = 0; vi <= order - ui; ++vi)
505  {
506  // Compute displacement along normal
507  d.z += u_pow (ui) * v_pow (vi) * c_vec[j];
508 
509  // Compute partial derivatives
510  if (ui >= 1)
511  d.z_u += c_vec[j] * ui * u_pow (ui - 1) * v_pow (vi);
512 
513  if (vi >= 1)
514  d.z_v += c_vec[j] * vi * u_pow (ui) * v_pow (vi - 1);
515 
516  if (ui >= 1 && vi >= 1)
517  d.z_uv += c_vec[j] * ui * u_pow (ui - 1) * vi * v_pow (vi - 1);
518 
519  if (ui >= 2)
520  d.z_uu += c_vec[j] * ui * (ui - 1) * u_pow (ui - 2) * v_pow (vi);
521 
522  if (vi >= 2)
523  d.z_vv += c_vec[j] * vi * (vi - 1) * u_pow (ui) * v_pow (vi - 2);
524 
525  if (ui == 0)
526  v_pow (vi + 1) = v_pow (vi) * v;
527 
528  ++j;
529  }
530  u_pow (ui + 1) = u_pow (ui) * u;
531  }
532 
533  return (d);
534 }
535 
537 pcl::MLSResult::projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const
538 {
539  double gu = u;
540  double gv = v;
541  double gw = 0;
542 
543  MLSProjectionResults result;
544  result.normal = plane_normal;
545  if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
546  {
547  PolynomialPartialDerivative d = getPolynomialPartialDerivative (gu, gv);
548  gw = d.z;
549  double err_total;
550  const double dist1 = std::abs (gw - w);
551  double dist2;
552  do
553  {
554  double e1 = (gu - u) + d.z_u * gw - d.z_u * w;
555  double e2 = (gv - v) + d.z_v * gw - d.z_v * w;
556 
557  const double F1u = 1 + d.z_uu * gw + d.z_u * d.z_u - d.z_uu * w;
558  const double F1v = d.z_uv * gw + d.z_u * d.z_v - d.z_uv * w;
559 
560  const double F2u = d.z_uv * gw + d.z_v * d.z_u - d.z_uv * w;
561  const double F2v = 1 + d.z_vv * gw + d.z_v * d.z_v - d.z_vv * w;
562 
563  Eigen::MatrixXd J (2, 2);
564  J (0, 0) = F1u;
565  J (0, 1) = F1v;
566  J (1, 0) = F2u;
567  J (1, 1) = F2v;
568 
569  Eigen::Vector2d err (e1, e2);
570  Eigen::Vector2d update = J.inverse () * err;
571  gu -= update (0);
572  gv -= update (1);
573 
574  d = getPolynomialPartialDerivative (gu, gv);
575  gw = d.z;
576  dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));
577 
578  err_total = std::sqrt (e1 * e1 + e2 * e2);
579 
580  } while (err_total > 1e-8 && dist2 < dist1);
581 
582  if (dist2 > dist1) // the optimization was diverging reset the coordinates for simple projection
583  {
584  gu = u;
585  gv = v;
586  d = getPolynomialPartialDerivative (u, v);
587  gw = d.z;
588  }
589 
590  result.u = gu;
591  result.v = gv;
592  result.normal -= (d.z_u * u_axis + d.z_v * v_axis);
593  result.normal.normalize ();
594  }
595 
596  result.point = mean + gu * u_axis + gv * v_axis + gw * plane_normal;
597 
598  return (result);
599 }
600 
602 pcl::MLSResult::projectPointToMLSPlane (const double u, const double v) const
603 {
604  MLSProjectionResults result;
605  result.u = u;
606  result.v = v;
607  result.normal = plane_normal;
608  result.point = mean + u * u_axis + v * v_axis;
609 
610  return (result);
611 }
612 
614 pcl::MLSResult::projectPointSimpleToPolynomialSurface (const double u, const double v) const
615 {
616  MLSProjectionResults result;
617  double w = 0;
618 
619  result.u = u;
620  result.v = v;
621  result.normal = plane_normal;
622 
623  if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
624  {
625  const PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v);
626  w = d.z;
627  result.normal -= (d.z_u * u_axis + d.z_v * v_axis);
628  result.normal.normalize ();
629  }
630 
631  result.point = mean + u * u_axis + v * v_axis + w * plane_normal;
632 
633  return (result);
634 }
635 
637 pcl::MLSResult::projectPoint (const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors) const
638 {
639  double u, v, w;
640  getMLSCoordinates (pt, u, v, w);
641 
643  if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
644  {
645  if (method == ORTHOGONAL)
646  proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
647  else // SIMPLE
648  proj = projectPointSimpleToPolynomialSurface (u, v);
649  }
650  else
651  {
652  proj = projectPointToMLSPlane (u, v);
653  }
654 
655  return (proj);
656 }
657 
659 pcl::MLSResult::projectQueryPoint (ProjectionMethod method, int required_neighbors) const
660 {
662  if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
663  {
664  if (method == ORTHOGONAL)
665  {
666  double u, v, w;
667  getMLSCoordinates (query_point, u, v, w);
668  proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
669  }
670  else // SIMPLE
671  {
672  // Projection onto MLS surface along Darboux normal to the height at (0,0)
673  proj.point = mean + (c_vec[0] * plane_normal);
674 
675  // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
676  proj.normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis;
677  proj.normal.normalize ();
678  }
679  }
680  else
681  {
682  proj.normal = plane_normal;
683  proj.point = mean;
684  }
685 
686  return (proj);
687 }
688 
689 template <typename PointT> void
691  pcl::index_t index,
692  const pcl::Indices &nn_indices,
693  double search_radius,
694  int polynomial_order,
695  std::function<double(const double)> weight_func)
696 {
697  // Compute the plane coefficients
698  EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
699  Eigen::Vector4d xyz_centroid;
700 
701  // Estimate the XYZ centroid
702  pcl::compute3DCentroid (cloud, nn_indices, xyz_centroid);
703 
704  // Compute the 3x3 covariance matrix
705  pcl::computeCovarianceMatrix (cloud, nn_indices, xyz_centroid, covariance_matrix);
706  EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
707  EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
708  Eigen::Vector4d model_coefficients (0, 0, 0, 0);
709  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
710  model_coefficients.head<3> ().matrix () = eigen_vector;
711  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
712 
713  query_point = cloud[index].getVector3fMap ().template cast<double> ();
714 
715  if (!std::isfinite(eigen_vector[0]) || !std::isfinite(eigen_vector[1]) || !std::isfinite(eigen_vector[2]))
716  {
717  // Invalid plane coefficients, this may happen if the input cloud is non-dense (it contains invalid points).
718  // Keep the input point and stop here.
719  valid = false;
720  mean = query_point;
721  return;
722  }
723 
724  // Projected query point
725  valid = true;
726  const double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
727  mean = query_point - distance * model_coefficients.head<3> ();
728 
729  curvature = covariance_matrix.trace ();
730  // Compute the curvature surface change
731  if (curvature != 0)
732  curvature = std::abs (eigen_value / curvature);
733 
734  // Get a copy of the plane normal easy access
735  plane_normal = model_coefficients.head<3> ();
736 
737  // Local coordinate system (Darboux frame)
738  v_axis = plane_normal.unitOrthogonal ();
739  u_axis = plane_normal.cross (v_axis);
740 
741  // Perform polynomial fit to update point and normal
742  ////////////////////////////////////////////////////
743  num_neighbors = static_cast<int> (nn_indices.size ());
744  order = polynomial_order;
745  if (order > 1)
746  {
747  const int nr_coeff = (order + 1) * (order + 2) / 2;
748 
749  if (num_neighbors >= nr_coeff)
750  {
751  if (!weight_func)
752  weight_func = [this, search_radius] (const double sq_dist) { return this->computeMLSWeight (sq_dist, search_radius * search_radius); };
753 
754  // Allocate matrices and vectors to hold the data used for the polynomial fit
755  Eigen::VectorXd weight_vec (num_neighbors);
756  Eigen::MatrixXd P (nr_coeff, num_neighbors);
757  Eigen::VectorXd f_vec (num_neighbors);
758  Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);
759 
760  // Update neighborhood, since point was projected, and computing relative
761  // positions. Note updating only distances for the weights for speed
762  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (num_neighbors);
763  for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
764  {
765  de_meaned[ni][0] = cloud[nn_indices[ni]].x - mean[0];
766  de_meaned[ni][1] = cloud[nn_indices[ni]].y - mean[1];
767  de_meaned[ni][2] = cloud[nn_indices[ni]].z - mean[2];
768  weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
769  }
770 
771  // Go through neighbors, transform them in the local coordinate system,
772  // save height and the evaluation of the polynomial's terms
773  for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
774  {
775  // Transforming coordinates
776  const double u_coord = de_meaned[ni].dot(u_axis);
777  const double v_coord = de_meaned[ni].dot(v_axis);
778  f_vec (ni) = de_meaned[ni].dot (plane_normal);
779 
780  // Compute the polynomial's terms at the current point
781  int j = 0;
782  double u_pow = 1;
783  for (int ui = 0; ui <= order; ++ui)
784  {
785  double v_pow = 1;
786  for (int vi = 0; vi <= order - ui; ++vi)
787  {
788  P (j++, ni) = u_pow * v_pow;
789  v_pow *= v_coord;
790  }
791  u_pow *= u_coord;
792  }
793  }
794 
795  // Computing coefficients
796  const Eigen::MatrixXd P_weight = P * weight_vec.asDiagonal(); // size will be (nr_coeff_, nn_indices.size ());
797  P_weight_Pt = P_weight * P.transpose ();
798  c_vec = P_weight * f_vec;
799  P_weight_Pt.llt ().solveInPlace (c_vec);
800  }
801  }
802 }
803 
804 //////////////////////////////////////////////////////////////////////////////////////////////
805 template <typename PointInT, typename PointOutT>
807  IndicesPtr &indices,
808  float voxel_size,
809  int dilation_iteration_num) :
810  voxel_grid_ (), data_size_ (), voxel_size_ (voxel_size)
811 {
812  pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);
813  bounding_min_ -= Eigen::Vector4f::Constant(voxel_size_ * (dilation_iteration_num + 1));
814  bounding_max_ += Eigen::Vector4f::Constant(voxel_size_ * (dilation_iteration_num + 1));
815 
816  Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;
817  const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
818  // Put initial cloud in voxel grid
819  data_size_ = static_cast<std::uint64_t> (std::ceil(max_size / voxel_size_));
820  for (std::size_t i = 0; i < indices->size (); ++i)
821  if (std::isfinite ((*cloud)[(*indices)[i]].x))
822  {
823  Eigen::Vector3i pos;
824  getCellIndex ((*cloud)[(*indices)[i]].getVector3fMap (), pos);
825 
826  std::uint64_t index_1d;
827  getIndexIn1D (pos, index_1d);
828  Leaf leaf;
829  voxel_grid_[index_1d] = leaf;
830  }
831 }
832 
833 //////////////////////////////////////////////////////////////////////////////////////////////
834 template <typename PointInT, typename PointOutT> void
836 {
837  HashMap new_voxel_grid = voxel_grid_;
838  for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
839  {
840  Eigen::Vector3i index;
841  getIndexIn3D (m_it->first, index);
842 
843  // Now dilate all of its voxels
844  for (int x = -1; x <= 1; ++x)
845  for (int y = -1; y <= 1; ++y)
846  for (int z = -1; z <= 1; ++z)
847  if (x != 0 || y != 0 || z != 0)
848  {
849  Eigen::Vector3i new_index;
850  new_index = index + Eigen::Vector3i (x, y, z);
851 
852  std::uint64_t index_1d;
853  getIndexIn1D (new_index, index_1d);
854  Leaf leaf;
855  new_voxel_grid[index_1d] = leaf;
856  }
857  }
858  voxel_grid_ = new_voxel_grid;
859 }
860 
861 
862 /////////////////////////////////////////////////////////////////////////////////////////////
863 template <typename PointInT, typename PointOutT> void
865  PointOutT &point_out) const
866 {
867  PointOutT temp = point_out;
868  copyPoint (point_in, point_out);
869  point_out.x = temp.x;
870  point_out.y = temp.y;
871  point_out.z = temp.z;
872 }
873 
874 #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;
875 #define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>;
876 
877 #endif // PCL_SURFACE_IMPL_MLS_H_
Define methods for centroid estimation and covariance matrix calculus.
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
Definition: mls.h:588
void getPosition(const std::uint64_t &index_1d, Eigen::Vector3f &point) const
Definition: mls.h:625
Eigen::Vector4f bounding_min_
Definition: mls.h:635
void getIndexIn1D(const Eigen::Vector3i &index, std::uint64_t &index_1d) const
Definition: mls.h:601
Eigen::Vector4f bounding_max_
Definition: mls.h:635
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
Definition: mls.h:618
MLSVoxelGrid(PointCloudInConstPtr &cloud, IndicesPtr &indices, float voxel_size, int dilation_iteration_num)
Definition: mls.hpp:806
std::map< std::uint64_t, Leaf > HashMap
Definition: mls.h:633
void performUpsampling(PointCloudOut &output)
Perform upsampling for the distinct-cloud and voxel-grid methods.
Definition: mls.hpp:370
typename KdTree::Ptr KdTreePtr
Definition: mls.h:275
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: mls.h:285
void copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
Definition: mls.hpp:864
void performProcessing(PointCloudOut &output) override
Abstract surface reconstruction method.
Definition: mls.hpp:284
void computeMLSPointNormal(pcl::index_t index, const pcl::Indices &nn_indices, 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:174
void process(PointCloudOut &output) override
Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()>
Definition: mls.hpp:61
void addProjectedPointNormal(pcl::index_t index, const Eigen::Vector3d &point, const Eigen::Vector3d &normal, double curvature, PointCloudOut &projected_points, NormalCloud &projected_points_normals, PointIndices &corresponding_input_indices) const
This is a helper function for adding projected points.
Definition: mls.hpp:252
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:663
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
Definition: point_cloud.h:713
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::vector< PointCloud< PointOutT >, Eigen::aligned_allocator< PointCloud< PointOutT > > > CloudVectorType
Definition: point_cloud.h:412
iterator end() noexcept
Definition: point_cloud.h:430
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
std::size_t size() const
Definition: point_cloud.h:443
iterator begin() noexcept
Definition: point_cloud.h:429
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Definition: organized.h:61
Define standard C methods and C++ classes that are common to all methods.
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:295
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:137
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.
Definition: centroid.hpp:191
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:296
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:56
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
Data structure used to store the MLS projection results.
Definition: mls.h:81
Eigen::Vector3d point
The projected point.
Definition: mls.h:86
double v
The v-coordinate of the projected point in local MLS frame.
Definition: mls.h:85
Eigen::Vector3d normal
The projected point's normal.
Definition: mls.h:87
double u
The u-coordinate of the projected point in local MLS frame.
Definition: mls.h:84
Data structure used to store the MLS polynomial partial derivatives.
Definition: mls.h:70
double z_uv
The partial derivative d^2z/dudv.
Definition: mls.h:76
double z_u
The partial derivative dz/du.
Definition: mls.h:72
double z_uu
The partial derivative d^2z/du^2.
Definition: mls.h:74
double z
The z component of the polynomial evaluated at z(u, v).
Definition: mls.h:71
double z_vv
The partial derivative d^2z/dv^2.
Definition: mls.h:75
double z_v
The partial derivative dz/dv.
Definition: mls.h:73
Data structure used to store the results of the MLS fitting.
Definition: mls.h:60
MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors=0) const
Project a point using the specified method.
Definition: mls.hpp:637
MLSResult()
Definition: mls.h:92
MLSProjectionResults projectPointOrthogonalToPolynomialSurface(const double u, const double v, const double w) const
Project a point orthogonal to the polynomial surface.
Definition: mls.hpp:537
ProjectionMethod
Definition: mls.h:62
int num_neighbors
The number of neighbors used to create the mls surface.
Definition: mls.h:231
void computeMLSSurface(const pcl::PointCloud< PointT > &cloud, pcl::index_t index, const pcl::Indices &nn_indices, double search_radius, int polynomial_order=2, std::function< double(const double)> weight_func={})
Smooth a given point and its neighborhood using Moving Least Squares.
Definition: mls.hpp:690
void getMLSCoordinates(const Eigen::Vector3d &pt, double &u, double &v, double &w) const
Given a point calculate its 3D location in the MLS frame.
Definition: mls.hpp:453
float curvature
The curvature at the query point.
Definition: mls.h:232
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial's first and second partial derivatives.
Definition: mls.hpp:492
MLSProjectionResults projectPointSimpleToPolynomialSurface(const double u, const double v) const
Project a point along the MLS plane normal to the polynomial surface.
Definition: mls.hpp:614
MLSProjectionResults projectPointToMLSPlane(const double u, const double v) const
Project a point onto the MLS plane.
Definition: mls.hpp:602
double getPolynomialValue(const double u, const double v) const
Calculate the polynomial.
Definition: mls.hpp:470
MLSProjectionResults projectQueryPoint(ProjectionMethod method, int required_neighbors=0) const
Project the query point used to generate the mls surface about using the specified method.
Definition: mls.hpp:659
A point structure representing normal coordinates and the surface curvature estimate.
A helper functor that can set a specific value in a field if the field exists.
Definition: type_traits.h:190