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