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