Point Cloud Library (PCL)  1.9.1-dev
mls.h
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 #pragma once
41 
42 #include <map>
43 #include <random>
44 
45 #include <boost/function.hpp>
46 
47 // PCL includes
48 #include <pcl/pcl_base.h>
49 #include <pcl/search/pcl_search.h>
50 #include <pcl/common/common.h>
51 
52 #include <pcl/surface/boost.h>
53 #include <pcl/surface/eigen.h>
54 #include <pcl/surface/processing.h>
55 
56 namespace pcl
57 {
58 
59  /** \brief Data structure used to store the results of the MLS fitting */
60  struct MLSResult
61  {
63  {
64  NONE, /**< \brief Project to the mls plane. */
65  SIMPLE, /**< \brief Project along the mls plane normal to the polynomial surface. */
66  ORTHOGONAL /**< \brief Project to the closest point on the polynonomial surface. */
67  };
68 
69  /** \brief Data structure used to store the MLS polynomial partial derivatives */
71  {
72  double z; /**< \brief The z component of the polynomial evaluated at z(u, v). */
73  double z_u; /**< \brief The partial derivative dz/du. */
74  double z_v; /**< \brief The partial derivative dz/dv. */
75  double z_uu; /**< \brief The partial derivative d^2z/du^2. */
76  double z_vv; /**< \brief The partial derivative d^2z/dv^2. */
77  double z_uv; /**< \brief The partial derivative d^2z/dudv. */
78  };
79 
80  /** \brief Data structure used to store the MLS projection results */
82  {
83  MLSProjectionResults () : u (0), v (0) {}
84 
85  double u; /**< \brief The u-coordinate of the projected point in local MLS frame. */
86  double v; /**< \brief The u-coordinate of the projected point in local MLS frame. */
87  Eigen::Vector3d point; /**< \brief The projected point. */
88  Eigen::Vector3d normal; /**< \brief The projected point's normal. */
89  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
90  };
91 
92  inline
93  MLSResult () : num_neighbors (0), curvature (0.0f), order (0), valid (false) {}
94 
95  inline
96  MLSResult (const Eigen::Vector3d &a_query_point,
97  const Eigen::Vector3d &a_mean,
98  const Eigen::Vector3d &a_plane_normal,
99  const Eigen::Vector3d &a_u,
100  const Eigen::Vector3d &a_v,
101  const Eigen::VectorXd &a_c_vec,
102  const int a_num_neighbors,
103  const float a_curvature,
104  const int a_order);
105 
106  /** \brief Given a point calculate it's 3D location in the MLS frame.
107  * \param[in] pt The point
108  * \param[out] u The u-coordinate of the point in local MLS frame.
109  * \param[out] v The v-coordinate of the point in local MLS frame.
110  * \param[out] w The w-coordinate of the point in local MLS frame.
111  */
112  inline void
113  getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v, double &w) const;
114 
115  /** \brief Given a point calculate it's 2D location in the MLS frame.
116  * \param[in] pt The point
117  * \param[out] u The u-coordinate of the point in local MLS frame.
118  * \param[out] v The v-coordinate of the point in local MLS frame.
119  */
120  inline void
121  getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v) const;
122 
123  /** \brief Calculate the polynomial
124  * \param[in] u The u-coordinate of the point in local MLS frame.
125  * \param[in] v The v-coordinate of the point in local MLS frame.
126  * \return The polynomial value at the provide uv coordinates.
127  */
128  inline double
129  getPolynomialValue (const double u, const double v) const;
130 
131  /** \brief Calculate the polynomial's first and second partial derivatives.
132  * \param[in] u The u-coordinate of the point in local MLS frame.
133  * \param[in] v The v-coordinate of the point in local MLS frame.
134  * \return The polynomial partial derivatives at the provide uv coordinates.
135  */
137  getPolynomialPartialDerivative (const double u, const double v) const;
138 
139  /** \brief Calculate the principle curvatures using the polynomial surface.
140  * \param[in] u The u-coordinate of the point in local MLS frame.
141  * \param[in] v The v-coordinate of the point in local MLS frame.
142  * \return The principle curvature [k1, k2] at the provided ub coordinates.
143  * \note If an error occurs the MLS_MINIMUM_PRINCIPLE_CURVATURE is returned.
144  */
145  inline Eigen::Vector2f
146  calculatePrincipleCurvatures (const double u, const double v) const;
147 
148  /** \brief Project a point orthogonal to the polynomial surface.
149  * \param[in] u The u-coordinate of the point in local MLS frame.
150  * \param[in] v The v-coordinate of the point in local MLS frame.
151  * \param[in] w The w-coordinate of the point in local MLS frame.
152  * \return The MLSProjectionResults for the input data.
153  * \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane.
154  * \note If the optimization diverges it performs a simple projection on to the polynomial surface.
155  * \note This was implemented based on this https://math.stackexchange.com/questions/1497093/shortest-distance-between-point-and-surface
156  */
157  inline MLSProjectionResults
158  projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const;
159 
160  /** \brief Project a point onto the MLS plane.
161  * \param[in] u The u-coordinate of the point in local MLS frame.
162  * \param[in] v The v-coordinate of the point in local MLS frame.
163  * \return The MLSProjectionResults for the input data.
164  */
165  inline MLSProjectionResults
166  projectPointToMLSPlane (const double u, const double v) const;
167 
168  /** \brief Project a point along the MLS plane normal to the polynomial surface.
169  * \param[in] u The u-coordinate of the point in local MLS frame.
170  * \param[in] v The v-coordinate of the point in local MLS frame.
171  * \return The MLSProjectionResults for the input data.
172  * \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane.
173  */
174  inline MLSProjectionResults
175  projectPointSimpleToPolynomialSurface (const double u, const double v) const;
176 
177  /**
178  * \brief Project a point using the specified method.
179  * \param[in] pt The point to be project.
180  * \param[in] method The projection method to be used.
181  * \param[in] required_neighbors The minimum number of neighbors required.
182  * \note If required_neighbors then any number of neighbors is allowed.
183  * \note If required_neighbors is not satisfied it projects to the mls plane.
184  * \return The MLSProjectionResults for the input data.
185  */
186  inline MLSProjectionResults
187  projectPoint (const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors = 0) const;
188 
189  /**
190  * \brief Project the query point used to generate the mls surface about using the specified method.
191  * \param[in] method The projection method to be used.
192  * \param[in] required_neighbors The minimum number of neighbors required.
193  * \note If required_neighbors then any number of neighbors is allowed.
194  * \note If required_neighbors is not satisfied it projects to the mls plane.
195  * \return The MLSProjectionResults for the input data.
196  */
197  inline MLSProjectionResults
198  projectQueryPoint (ProjectionMethod method, int required_neighbors = 0) const;
199 
200  /** \brief Smooth a given point and its neighborghood using Moving Least Squares.
201  * \param[in] index the index of the query point in the input cloud
202  * \param[in] nn_indices the set of nearest neighbors indices for pt
203  * \param[in] search_radius the search radius used to find nearest neighbors for pt
204  * \param[in] polynomial_order the order of the polynomial to fit to the nearest neighbors
205  * \param[in] weight_func defines the weight function for the polynomial fit
206  */
207  template <typename PointT> void
209  int index,
210  const std::vector<int> &nn_indices,
211  double search_radius,
212  int polynomial_order = 2,
213  boost::function<double(const double)> weight_func = {});
214 
215  Eigen::Vector3d query_point; /**< \brief The query point about which the mls surface was generated */
216  Eigen::Vector3d mean; /**< \brief The mean point of all the neighbors. */
217  Eigen::Vector3d plane_normal; /**< \brief The normal of the local plane of the query point. */
218  Eigen::Vector3d u_axis; /**< \brief The axis corresponding to the u-coordinates of the local plane of the query point. */
219  Eigen::Vector3d v_axis; /**< \brief The axis corresponding to the v-coordinates of the local plane of the query point. */
220  Eigen::VectorXd c_vec; /**< \brief The polynomial coefficients Example: z = c_vec[0] + c_vec[1]*v + c_vec[2]*v^2 + c_vec[3]*u + c_vec[4]*u*v + c_vec[5]*u^2 */
221  int num_neighbors; /**< \brief The number of neighbors used to create the mls surface. */
222  float curvature; /**< \brief The curvature at the query point. */
223  int order; /**< \brief The order of the polynomial. If order > 1 then use polynomial fit */
224  bool valid; /**< \brief If True, the mls results data is valid, otherwise False. */
225  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
226  private:
227  /**
228  * \brief The default weight function used when fitting a polynomial surface
229  * \param sq_dist the squared distance from a point to origin of the mls frame
230  * \param sq_mls_radius the squraed mls search radius used
231  * \return The weight for a point at squared distance from the origin of the mls frame
232  */
233  inline
234  double computeMLSWeight (const double sq_dist, const double sq_mls_radius) { return (exp (-sq_dist / sq_mls_radius)); }
235 
236  };
237 
238  /** \brief MovingLeastSquares represent an implementation of the MLS (Moving Least Squares) algorithm
239  * for data smoothing and improved normal estimation. It also contains methods for upsampling the
240  * resulting cloud based on the parametric fit.
241  * Reference paper: "Computing and Rendering Point Set Surfaces" by Marc Alexa, Johannes Behr,
242  * Daniel Cohen-Or, Shachar Fleishman, David Levin and Claudio T. Silva
243  * www.sci.utah.edu/~shachar/Publications/crpss.pdf
244  * \note There is a parallelized version of the processing step, using the OpenMP standard.
245  * Compared to the standard version, an overhead is incurred in terms of runtime and memory usage.
246  * The upsampling methods DISTINCT_CLOUD and VOXEL_GRID_DILATION are not parallelized completely,
247  * i.e. parts of the algorithm run on a single thread only.
248  * \author Zoltan Csaba Marton, Radu B. Rusu, Alexandru E. Ichim, Suat Gedikli, Robert Huitl
249  * \ingroup surface
250  */
251  template <typename PointInT, typename PointOutT>
252  class MovingLeastSquares : public CloudSurfaceProcessing<PointInT, PointOutT>
253  {
254  public:
255  typedef boost::shared_ptr<MovingLeastSquares<PointInT, PointOutT> > Ptr;
256  typedef boost::shared_ptr<const MovingLeastSquares<PointInT, PointOutT> > ConstPtr;
257 
263 
265  typedef typename KdTree::Ptr KdTreePtr;
267  typedef NormalCloud::Ptr NormalCloudPtr;
268 
272 
276 
277  typedef boost::function<int (int, double, std::vector<int> &, std::vector<float> &)> SearchMethod;
278 
280  {
281  NONE, /**< \brief No upsampling will be done, only the input points will be projected
282  to their own MLS surfaces. */
283  DISTINCT_CLOUD, /**< \brief Project the points of the distinct cloud to the MLS surface. */
284  SAMPLE_LOCAL_PLANE, /**< \brief The local plane of each input point will be sampled in a circular fashion
285  using the \ref upsampling_radius_ and the \ref upsampling_step_ parameters. */
286  RANDOM_UNIFORM_DENSITY, /**< \brief The local plane of each input point will be sampled using an uniform random
287  distribution such that the density of points is constant throughout the
288  cloud - given by the \ref desired_num_points_in_radius_ parameter. */
289  VOXEL_GRID_DILATION /**< \brief The input cloud will be inserted into a voxel grid with voxels of
290  size \ref voxel_size_; this voxel grid will be dilated \ref dilation_iteration_num_
291  times and the resulting points will be projected to the MLS surface
292  of the closest point in the input cloud; the result is a point cloud
293  with filled holes and a constant point density. */
294  };
295 
296  /** \brief Empty constructor. */
297  MovingLeastSquares () : CloudSurfaceProcessing<PointInT, PointOutT> (),
298  distinct_cloud_ (),
299  tree_ (),
300  order_ (2),
301  search_radius_ (0.0),
302  sqr_gauss_param_ (0.0),
303  compute_normals_ (false),
304  upsample_method_ (NONE),
305  upsampling_radius_ (0.0),
306  upsampling_step_ (0.0),
307  desired_num_points_in_radius_ (0),
308  cache_mls_results_ (true),
309  projection_method_ (MLSResult::SIMPLE),
310  threads_ (1),
311  voxel_size_ (1.0),
312  dilation_iteration_num_ (0),
313  nr_coeff_ (),
314  rng_uniform_distribution_ ()
315  {};
316 
317  /** \brief Empty destructor */
319 
320 
321  /** \brief Set whether the algorithm should also store the normals computed
322  * \note This is optional, but need a proper output cloud type
323  */
324  inline void
325  setComputeNormals (bool compute_normals) { compute_normals_ = compute_normals; }
326 
327  /** \brief Provide a pointer to the search object.
328  * \param[in] tree a pointer to the spatial search object.
329  */
330  inline void
331  setSearchMethod (const KdTreePtr &tree)
332  {
333  tree_ = tree;
334  // Declare the search locator definition
335  int (KdTree::*radiusSearch)(int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances, unsigned int max_nn) const = &KdTree::radiusSearch;
336  search_method_ = boost::bind (radiusSearch, boost::ref (tree_), _1, _2, _3, _4, 0);
337  }
338 
339  /** \brief Get a pointer to the search method used. */
340  inline KdTreePtr
341  getSearchMethod () const { return (tree_); }
342 
343  /** \brief Set the order of the polynomial to be fit.
344  * \param[in] order the order of the polynomial
345  * \note Setting order > 1 indicates using a polynomial fit.
346  */
347  inline void
348  setPolynomialOrder (int order) { order_ = order; }
349 
350  /** \brief Get the order of the polynomial to be fit. */
351  inline int
352  getPolynomialOrder () const { return (order_); }
353 
354  /** \brief Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimation.
355  * \param[in] polynomial_fit set to true for polynomial fit
356  */
357  [[deprecated("use setPolynomialOrder() instead")]]
358  inline void
359  setPolynomialFit (bool polynomial_fit)
360  {
361  if (polynomial_fit)
362  {
363  if (order_ < 2)
364  {
365  order_ = 2;
366  }
367  }
368  else
369  {
370  order_ = 0;
371  }
372  }
373 
374  /** \brief Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial). */
375  [[deprecated("use getPolynomialOrder() instead")]]
376  inline bool
377  getPolynomialFit () const { return (order_ > 1); }
378 
379  /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting.
380  * \param[in] radius the sphere radius that is to contain all k-nearest neighbors
381  * \note Calling this method resets the squared Gaussian parameter to radius * radius !
382  */
383  inline void
384  setSearchRadius (double radius) { search_radius_ = radius; sqr_gauss_param_ = search_radius_ * search_radius_; }
385 
386  /** \brief Get the sphere radius used for determining the k-nearest neighbors. */
387  inline double
388  getSearchRadius () const { return (search_radius_); }
389 
390  /** \brief Set the parameter used for distance based weighting of neighbors (the square of the search radius works
391  * best in general).
392  * \param[in] sqr_gauss_param the squared Gaussian parameter
393  */
394  inline void
395  setSqrGaussParam (double sqr_gauss_param) { sqr_gauss_param_ = sqr_gauss_param; }
396 
397  /** \brief Get the parameter for distance based weighting of neighbors. */
398  inline double
399  getSqrGaussParam () const { return (sqr_gauss_param_); }
400 
401  /** \brief Set the upsampling method to be used
402  * \param method
403  */
404  inline void
405  setUpsamplingMethod (UpsamplingMethod method) { upsample_method_ = method; }
406 
407  /** \brief Set the distinct cloud used for the DISTINCT_CLOUD upsampling method. */
408  inline void
409  setDistinctCloud (PointCloudInConstPtr distinct_cloud) { distinct_cloud_ = distinct_cloud; }
410 
411  /** \brief Get the distinct cloud used for the DISTINCT_CLOUD upsampling method. */
412  inline PointCloudInConstPtr
413  getDistinctCloud () const { return (distinct_cloud_); }
414 
415 
416  /** \brief Set the radius of the circle in the local point plane that will be sampled
417  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
418  * \param[in] radius the radius of the circle
419  */
420  inline void
421  setUpsamplingRadius (double radius) { upsampling_radius_ = radius; }
422 
423  /** \brief Get the radius of the circle in the local point plane that will be sampled
424  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
425  */
426  inline double
427  getUpsamplingRadius () const { return (upsampling_radius_); }
428 
429  /** \brief Set the step size for the local plane sampling
430  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
431  * \param[in] step_size the step size
432  */
433  inline void
434  setUpsamplingStepSize (double step_size) { upsampling_step_ = step_size; }
435 
436 
437  /** \brief Get the step size for the local plane sampling
438  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
439  */
440  inline double
441  getUpsamplingStepSize () const { return (upsampling_step_); }
442 
443  /** \brief Set the parameter that specifies the desired number of points within the search radius
444  * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
445  * \param[in] desired_num_points_in_radius the desired number of points in the output cloud in a sphere of
446  * radius \ref search_radius_ around each point
447  */
448  inline void
449  setPointDensity (int desired_num_points_in_radius) { desired_num_points_in_radius_ = desired_num_points_in_radius; }
450 
451 
452  /** \brief Get the parameter that specifies the desired number of points within the search radius
453  * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
454  */
455  inline int
456  getPointDensity () const { return (desired_num_points_in_radius_); }
457 
458  /** \brief Set the voxel size for the voxel grid
459  * \note Used only in the VOXEL_GRID_DILATION upsampling method
460  * \param[in] voxel_size the edge length of a cubic voxel in the voxel grid
461  */
462  inline void
463  setDilationVoxelSize (float voxel_size) { voxel_size_ = voxel_size; }
464 
465 
466  /** \brief Get the voxel size for the voxel grid
467  * \note Used only in the VOXEL_GRID_DILATION upsampling method
468  */
469  inline float
470  getDilationVoxelSize () const { return (voxel_size_); }
471 
472  /** \brief Set the number of dilation steps of the voxel grid
473  * \note Used only in the VOXEL_GRID_DILATION upsampling method
474  * \param[in] iterations the number of dilation iterations
475  */
476  inline void
477  setDilationIterations (int iterations) { dilation_iteration_num_ = iterations; }
478 
479  /** \brief Get the number of dilation steps of the voxel grid
480  * \note Used only in the VOXEL_GRID_DILATION upsampling method
481  */
482  inline int
483  getDilationIterations () const { return (dilation_iteration_num_); }
484 
485  /** \brief Set whether the mls results should be stored for each point in the input cloud
486  * \param[in] True if the mls results should be stored, otherwise false.
487  * \note The cache_mls_results_ is forced to true when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.
488  * \note If memory consumption is a concern set to false when not using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.
489  */
490  inline void
491  setCacheMLSResults (bool cache_mls_results) { cache_mls_results_ = cache_mls_results; }
492 
493  /** \brief Get the cache_mls_results_ value (True if the mls results should be stored, otherwise false). */
494  inline bool
495  getCacheMLSResults () const { return (cache_mls_results_); }
496 
497  /** \brief Set the method to be used when projection the point on to the MLS surface.
498  * \param method
499  * \note This is only used when polynomial fit is enabled.
500  */
501  inline void
502  setProjectionMethod (MLSResult::ProjectionMethod method) { projection_method_ = method; }
503 
504 
505  /** \brief Get the current projection method being used. */
507  getProjectionMethod () const { return (projection_method_); }
508 
509  /** \brief Get the MLSResults for input cloud
510  * \note The results are only stored if setCacheMLSResults(true) was called or when using the upsampling method DISTINCT_CLOUD or VOXEL_GRID_DILATION.
511  * \note This vector is align with the input cloud indices, so use getCorrespondingIndices to get the correct results when using output cloud indices.
512  */
513  inline const std::vector<MLSResult>&
514  getMLSResults () const { return (mls_results_); }
515 
516  /** \brief Set the maximum number of threads to use
517  * \param threads the maximum number of hardware threads to use (0 sets the value to 1)
518  */
519  inline void
520  setNumberOfThreads (unsigned int threads = 1)
521  {
522  threads_ = threads;
523  }
524 
525  /** \brief Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()>
526  * \param[out] output the resultant reconstructed surface model
527  */
528  void
529  process (PointCloudOut &output) override;
530 
531 
532  /** \brief Get the set of indices with each point in output having the
533  * corresponding point in input */
534  inline PointIndicesPtr
535  getCorrespondingIndices () const { return (corresponding_input_indices_); }
536 
537  protected:
538  /** \brief The point cloud that will hold the estimated normals, if set. */
539  NormalCloudPtr normals_;
540 
541  /** \brief The distinct point cloud that will be projected to the MLS surface. */
542  PointCloudInConstPtr distinct_cloud_;
543 
544  /** \brief The search method template for indices. */
546 
547  /** \brief A pointer to the spatial search object. */
548  KdTreePtr tree_;
549 
550  /** \brief The order of the polynomial to be fit. */
551  int order_;
552 
553  /** \brief The nearest neighbors search radius for each point. */
555 
556  /** \brief Parameter for distance based weighting of neighbors (search_radius_ * search_radius_ works fine) */
558 
559  /** \brief Parameter that specifies whether the normals should be computed for the input cloud or not */
561 
562  /** \brief Parameter that specifies the upsampling method to be used */
564 
565  /** \brief Radius of the circle in the local point plane that will be sampled
566  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
567  */
569 
570  /** \brief Step size for the local plane sampling
571  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
572  */
574 
575  /** \brief Parameter that specifies the desired number of points within the search radius
576  * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
577  */
579 
580  /** \brief True if the mls results for the input cloud should be stored
581  * \note This is forced to true when using upsampling methods VOXEL_GRID_DILATION or DISTINCT_CLOUD.
582  */
584 
585  /** \brief Stores the MLS result for each point in the input cloud
586  * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling
587  */
588  std::vector<MLSResult> mls_results_;
589 
590  /** \brief Parameter that specifies the projection method to be used. */
592 
593  /** \brief The maximum number of threads the scheduler should use. */
594  unsigned int threads_;
595 
596 
597  /** \brief A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling
598  * \note Used only in the case of VOXEL_GRID_DILATION upsampling
599  */
601  {
602  public:
603  struct Leaf { Leaf () : valid (true) {} bool valid; };
604 
605  MLSVoxelGrid (PointCloudInConstPtr& cloud,
606  IndicesPtr &indices,
607  float voxel_size);
608 
609  void
610  dilate ();
611 
612  inline void
613  getIndexIn1D (const Eigen::Vector3i &index, uint64_t &index_1d) const
614  {
615  index_1d = index[0] * data_size_ * data_size_ +
616  index[1] * data_size_ + index[2];
617  }
618 
619  inline void
620  getIndexIn3D (uint64_t index_1d, Eigen::Vector3i& index_3d) const
621  {
622  index_3d[0] = static_cast<Eigen::Vector3i::Scalar> (index_1d / (data_size_ * data_size_));
623  index_1d -= index_3d[0] * data_size_ * data_size_;
624  index_3d[1] = static_cast<Eigen::Vector3i::Scalar> (index_1d / data_size_);
625  index_1d -= index_3d[1] * data_size_;
626  index_3d[2] = static_cast<Eigen::Vector3i::Scalar> (index_1d);
627  }
628 
629  inline void
630  getCellIndex (const Eigen::Vector3f &p, Eigen::Vector3i& index) const
631  {
632  for (int i = 0; i < 3; ++i)
633  index[i] = static_cast<Eigen::Vector3i::Scalar> ((p[i] - bounding_min_ (i)) / voxel_size_);
634  }
635 
636  inline void
637  getPosition (const uint64_t &index_1d, Eigen::Vector3f &point) const
638  {
639  Eigen::Vector3i index_3d;
640  getIndexIn3D (index_1d, index_3d);
641  for (int i = 0; i < 3; ++i)
642  point[i] = static_cast<Eigen::Vector3f::Scalar> (index_3d[i]) * voxel_size_ + bounding_min_[i];
643  }
644 
645  typedef std::map<uint64_t, Leaf> HashMap;
646  HashMap voxel_grid_;
647  Eigen::Vector4f bounding_min_, bounding_max_;
648  uint64_t data_size_;
649  float voxel_size_;
650  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
651  };
652 
653 
654  /** \brief Voxel size for the VOXEL_GRID_DILATION upsampling method */
655  float voxel_size_;
656 
657  /** \brief Number of dilation steps for the VOXEL_GRID_DILATION upsampling method */
659 
660  /** \brief Number of coefficients, to be computed from the requested order.*/
662 
663  /** \brief Collects for each point in output the corrseponding point in the input. */
665 
666  /** \brief Search for the closest nearest neighbors of a given point using a radius search
667  * \param[in] index the index of the query point
668  * \param[out] indices the resultant vector of indices representing the k-nearest neighbors
669  * \param[out] sqr_distances the resultant squared distances from the query point to the k-nearest neighbors
670  */
671  inline int
672  searchForNeighbors (int index, std::vector<int> &indices, std::vector<float> &sqr_distances) const
673  {
674  return (search_method_ (index, search_radius_, indices, sqr_distances));
675  }
676 
677  /** \brief Smooth a given point and its neighborghood using Moving Least Squares.
678  * \param[in] index the index of the query point in the input cloud
679  * \param[in] nn_indices the set of nearest neighbors indices for pt
680  * \param[out] projected_points the set of points projected points around the query point
681  * (in the case of upsampling method NONE, only the query point projected to its own fitted surface will be returned,
682  * in the case of the other upsampling methods, multiple points will be returned)
683  * \param[out] projected_points_normals the normals corresponding to the projected points
684  * \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input
685  * \param[out] mls_result stores the MLS result for each point in the input cloud
686  * (used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling)
687  */
688  void
689  computeMLSPointNormal (int index,
690  const std::vector<int> &nn_indices,
691  PointCloudOut &projected_points,
692  NormalCloud &projected_points_normals,
693  PointIndices &corresponding_input_indices,
694  MLSResult &mls_result) const;
695 
696 
697  /** \brief This is a helper function for add projected points
698  * \param[in] index the index of the query point in the input cloud
699  * \param[in] point the projected point to be added
700  * \param[in] normal the projected point's normal to be added
701  * \param[in] curvature the projected point's curvature
702  * \param[out] projected_points the set of projected points around the query point
703  * \param[out] projected_points_normals the normals corresponding to the projected points
704  * \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input
705  */
706  void
707  addProjectedPointNormal (int index,
708  const Eigen::Vector3d &point,
709  const Eigen::Vector3d &normal,
710  double curvature,
711  PointCloudOut &projected_points,
712  NormalCloud &projected_points_normals,
713  PointIndices &corresponding_input_indices) const;
714 
715 
716  void
717  copyMissingFields (const PointInT &point_in,
718  PointOutT &point_out) const;
719 
720  /** \brief Abstract surface reconstruction method.
721  * \param[out] output the result of the reconstruction
722  */
723  void
724  performProcessing (PointCloudOut &output) override;
725 
726  /** \brief Perform upsampling for the distinct-cloud and voxel-grid methods
727  * \param[out] output the result of the reconstruction
728  */
729  void
730  performUpsampling (PointCloudOut &output);
731 
732  private:
733  /** \brief Random number generator algorithm. */
734  mutable std::mt19937 rng_;
735 
736  /** \brief Random number generator using an uniform distribution of floats
737  * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
738  */
739  std::unique_ptr<std::uniform_real_distribution<>> rng_uniform_distribution_;
740 
741  /** \brief Abstract class get name method. */
742  std::string
743  getClassName () const { return ("MovingLeastSquares"); }
744  };
745 
746  /** \brief MovingLeastSquaresOMP implementation has been merged into MovingLeastSquares for better maintainability.
747  * \note Keeping this empty child class for backwards compatibility.
748  * \author Robert Huitl
749  * \ingroup surface
750  */
751  template <typename PointInT, typename PointOutT>
752  class MovingLeastSquaresOMP : public MovingLeastSquares<PointInT, PointOutT>
753  {
754  public:
755  /** \brief Constructor for parallelized Moving Least Squares
756  * \param threads the maximum number of hardware threads to use (0 sets the value to 1)
757  */
758  MovingLeastSquaresOMP (unsigned int threads = 1)
759  {
760  this->setNumberOfThreads (threads);
761  }
762  };
763 }
764 
765 #ifdef PCL_NO_PRECOMPILE
766 #include <pcl/surface/impl/mls.hpp>
767 #endif
Data structure used to store the MLS polynomial partial derivatives.
Definition: mls.h:70
bool valid
If True, the mls results data is valid, otherwise False.
Definition: mls.h:224
virtual int radiusSearch(const PointT &p_q, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
int nr_coeff_
Number of coefficients, to be computed from the requested order.
Definition: mls.h:661
Eigen::Vector3d plane_normal
The normal of the local plane of the query point.
Definition: mls.h:217
PointCloudInConstPtr getDistinctCloud() const
Get the distinct cloud used for the DISTINCT_CLOUD upsampling method.
Definition: mls.h:413
int getPolynomialOrder() const
Get the order of the polynomial to be fit.
Definition: mls.h:352
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={})
Smooth a given point and its neighborghood using Moving Least Squares.
Definition: mls.hpp:717
double getSqrGaussParam() const
Get the parameter for distance based weighting of neighbors.
Definition: mls.h:399
pcl::PointCloud< pcl::Normal > NormalCloud
Definition: mls.h:266
double z_u
The partial derivative dz/du.
Definition: mls.h:73
const std::vector< MLSResult > & getMLSResults() const
Get the MLSResults for input cloud.
Definition: mls.h:514
MLSResult()
Definition: mls.h:93
PointCloudOut::Ptr PointCloudOutPtr
Definition: mls.h:270
double getSearchRadius() const
Get the sphere radius used for determining the k-nearest neighbors.
Definition: mls.h:388
NormalCloudPtr normals_
The point cloud that will hold the estimated normals, if set.
Definition: mls.h:539
void setDilationVoxelSize(float voxel_size)
Set the voxel size for the voxel grid.
Definition: mls.h:463
void setSearchRadius(double radius)
Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting...
Definition: mls.h:384
PointCloudIn::Ptr PointCloudInPtr
Definition: mls.h:274
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:220
int dilation_iteration_num_
Number of dilation steps for the VOXEL_GRID_DILATION upsampling method.
Definition: mls.h:658
double u
The u-coordinate of the projected point in local MLS frame.
Definition: mls.h:85
NormalCloud::Ptr NormalCloudPtr
Definition: mls.h:267
void setUpsamplingMethod(UpsamplingMethod method)
Set the upsampling method to be used.
Definition: mls.h:405
PointIndicesPtr getCorrespondingIndices() const
Get the set of indices with each point in output having the corresponding point in input...
Definition: mls.h:535
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< MovingLeastSquares< PointInT, PointOutT > > Ptr
Definition: mls.h:255
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
pcl::search::Search< PointInT > KdTree
Definition: mls.h:264
PointIndicesPtr corresponding_input_indices_
Collects for each point in output the corrseponding point in the input.
Definition: mls.h:664
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
MLSProjectionResults projectPointToMLSPlane(const double u, const double v) const
Project a point onto the MLS plane.
Definition: mls.hpp:629
void setPolynomialOrder(int order)
Set the order of the polynomial to be fit.
Definition: mls.h:348
MovingLeastSquares()
Empty constructor.
Definition: mls.h:297
MovingLeastSquares represent an implementation of the MLS (Moving Least Squares) algorithm for data s...
Definition: mls.h:252
ProjectionMethod
Definition: mls.h:62
double z_vv
The partial derivative d^2z/dv^2.
Definition: mls.h:76
~MovingLeastSquares()
Empty destructor.
Definition: mls.h:318
float getDilationVoxelSize() const
Get the voxel size for the voxel grid.
Definition: mls.h:470
SearchMethod search_method_
The search method template for indices.
Definition: mls.h:545
double z_uv
The partial derivative d^2z/dudv.
Definition: mls.h:77
int getPointDensity() const
Get the parameter that specifies the desired number of points within the search radius.
Definition: mls.h:456
void setCacheMLSResults(bool cache_mls_results)
Set whether the mls results should be stored for each point in the input cloud.
Definition: mls.h:491
MovingLeastSquaresOMP implementation has been merged into MovingLeastSquares for better maintainabili...
Definition: mls.h:752
CloudSurfaceProcessing represents the base class for algorithms that takes a point cloud as input and...
Definition: processing.h:56
pcl::PointCloud< PointInT > PointCloudIn
Definition: mls.h:273
boost::shared_ptr< PointIndices > PointIndicesPtr
Definition: pcl_base.h:75
double getUpsamplingStepSize() const
Get the step size for the local plane sampling.
Definition: mls.h:441
void setUpsamplingRadius(double radius)
Set the radius of the circle in the local point plane that will be sampled.
Definition: mls.h:421
Project to the closest point on the polynonomial surface.
Definition: mls.h:66
MovingLeastSquaresOMP(unsigned int threads=1)
Constructor for parallelized Moving Least Squares.
Definition: mls.h:758
boost::shared_ptr< const MovingLeastSquares< PointInT, PointOutT > > ConstPtr
Definition: mls.h:256
bool getPolynomialFit() const
Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial)...
Definition: mls.h:377
int order
The order of the polynomial.
Definition: mls.h:223
Eigen::Vector4f bounding_min_
Definition: mls.h:647
Project the points of the distinct cloud to the MLS surface.
Definition: mls.h:283
bool cache_mls_results_
True if the mls results for the input cloud should be stored.
Definition: mls.h:583
double z_uu
The partial derivative d^2z/du^2.
Definition: mls.h:75
boost::shared_ptr< PointCloud< PointOutT > > Ptr
Definition: point_cloud.h:427
std::vector< MLSResult > mls_results_
Stores the MLS result for each point in the input cloud.
Definition: mls.h:588
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:81
Eigen::Vector3d point
The projected point.
Definition: mls.h:87
double v
The u-coordinate of the projected point in local MLS frame.
Definition: mls.h:86
double z_v
The partial derivative dz/dv.
Definition: mls.h:74
pcl::PointCloud< PointOutT > PointCloudOut
Definition: mls.h:269
void setProjectionMethod(MLSResult::ProjectionMethod method)
Set the method to be used when projection the point on to the MLS surface.
Definition: mls.h:502
Eigen::Vector3d normal
The projected point&#39;s normal.
Definition: mls.h:88
PointCloudOut::ConstPtr PointCloudOutConstPtr
Definition: mls.h:271
double upsampling_radius_
Radius of the circle in the local point plane that will be sampled.
Definition: mls.h:568
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:80
MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors=0) const
Project a point using the specified method.
Definition: mls.hpp:664
KdTreePtr getSearchMethod() const
Get a pointer to the search method used.
Definition: mls.h:341
void getIndexIn3D(uint64_t index_1d, Eigen::Vector3i &index_3d) const
Definition: mls.h:620
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial&#39;s first and second partial derivatives.
Definition: mls.hpp:487
Data structure used to store the results of the MLS fitting.
Definition: mls.h:60
PointCloudInConstPtr distinct_cloud_
The distinct point cloud that will be projected to the MLS surface.
Definition: mls.h:542
PCL base class.
Definition: pcl_base.h:68
int num_neighbors
The number of neighbors used to create the mls surface.
Definition: mls.h:221
void setDistinctCloud(PointCloudInConstPtr distinct_cloud)
Set the distinct cloud used for the DISTINCT_CLOUD upsampling method.
Definition: mls.h:409
double search_radius_
The nearest neighbors search radius for each point.
Definition: mls.h:554
Eigen::Vector3d u_axis
The axis corresponding to the u-coordinates of the local plane of the query point.
Definition: mls.h:218
boost::shared_ptr< const PointCloud< PointOutT > > ConstPtr
Definition: point_cloud.h:428
void setUpsamplingStepSize(double step_size)
Set the step size for the local plane sampling.
Definition: mls.h:434
void setPolynomialFit(bool polynomial_fit)
Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimati...
Definition: mls.h:359
Eigen::Vector3d mean
The mean point of all the neighbors.
Definition: mls.h:216
double upsampling_step_
Step size for the local plane sampling.
Definition: mls.h:573
Eigen::Vector3d v_axis
The axis corresponding to the v-coordinates of the local plane of the query point.
Definition: mls.h:219
unsigned int threads_
The maximum number of threads the scheduler should use.
Definition: mls.h:594
Project to the mls plane.
Definition: mls.h:64
void setSqrGaussParam(double sqr_gauss_param)
Set the parameter used for distance based weighting of neighbors (the square of the search radius wor...
Definition: mls.h:395
PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: mls.h:275
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Eigen::Vector3d query_point
The query point about which the mls surface was generated.
Definition: mls.h:215
KdTreePtr tree_
A pointer to the spatial search object.
Definition: mls.h:548
int searchForNeighbors(int index, std::vector< int > &indices, std::vector< float > &sqr_distances) const
Search for the closest nearest neighbors of a given point using a radius search.
Definition: mls.h:672
int getDilationIterations() const
Get the number of dilation steps of the voxel grid.
Definition: mls.h:483
KdTree::Ptr KdTreePtr
Definition: mls.h:265
A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling.
Definition: mls.h:600
void getIndexIn1D(const Eigen::Vector3i &index, uint64_t &index_1d) const
Definition: mls.h:613
int order_
The order of the polynomial to be fit.
Definition: mls.h:551
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: mls.h:331
bool getCacheMLSResults() const
Get the cache_mls_results_ value (True if the mls results should be stored, otherwise false)...
Definition: mls.h:495
float voxel_size_
Voxel size for the VOXEL_GRID_DILATION upsampling method.
Definition: mls.h:655
bool compute_normals_
Parameter that specifies whether the normals should be computed for the input cloud or not...
Definition: mls.h:560
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
int desired_num_points_in_radius_
Parameter that specifies the desired number of points within the search radius.
Definition: mls.h:578
void setDilationIterations(int iterations)
Set the number of dilation steps of the voxel grid.
Definition: mls.h:477
void getPosition(const uint64_t &index_1d, Eigen::Vector3f &point) const
Definition: mls.h:637
boost::function< int(int, double, std::vector< int > &, std::vector< float > &)> SearchMethod
Definition: mls.h:277
float curvature
The curvature at the query point.
Definition: mls.h:222
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
MLSResult::ProjectionMethod projection_method_
Parameter that specifies the projection method to be used.
Definition: mls.h:591
void setPointDensity(int desired_num_points_in_radius)
Set the parameter that specifies the desired number of points within the search radius.
Definition: mls.h:449
void setComputeNormals(bool compute_normals)
Set whether the algorithm should also store the normals computed.
Definition: mls.h:325
UpsamplingMethod upsample_method_
Parameter that specifies the upsampling method to be used.
Definition: mls.h:563
double z
The z component of the polynomial evaluated at z(u, v).
Definition: mls.h:72
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
Definition: mls.h:630
MLSResult::ProjectionMethod getProjectionMethod() const
Get the current projection method being used.
Definition: mls.h:507
Eigen::Vector2f calculatePrincipleCurvatures(const double u, const double v) const
Calculate the principle curvatures using the polynomial surface.
Definition: mls.hpp:532
double getUpsamplingRadius() const
Get the radius of the circle in the local point plane that will be sampled.
Definition: mls.h:427
double sqr_gauss_param_
Parameter for distance based weighting of neighbors (search_radius_ * search_radius_ works fine) ...
Definition: mls.h:557
void setNumberOfThreads(unsigned int threads=1)
Set the maximum number of threads to use.
Definition: mls.h:520
Project along the mls plane normal to the polynomial surface.
Definition: mls.h:65
std::map< uint64_t, Leaf > HashMap
Definition: mls.h:645