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