Point Cloud Library (PCL)  1.7.1
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 #ifndef PCL_MLS_H_
41 #define PCL_MLS_H_
42 
43 // PCL includes
44 #include <pcl/pcl_base.h>
45 #include <pcl/search/pcl_search.h>
46 #include <pcl/common/common.h>
47 
48 #include <pcl/surface/boost.h>
49 #include <pcl/surface/eigen.h>
50 #include <pcl/surface/processing.h>
51 #include <map>
52 
53 namespace pcl
54 {
55  /** \brief MovingLeastSquares represent an implementation of the MLS (Moving Least Squares) algorithm
56  * for data smoothing and improved normal estimation. It also contains methods for upsampling the
57  * resulting cloud based on the parametric fit.
58  * Reference paper: "Computing and Rendering Point Set Surfaces" by Marc Alexa, Johannes Behr,
59  * Daniel Cohen-Or, Shachar Fleishman, David Levin and Claudio T. Silva
60  * www.sci.utah.edu/~shachar/Publications/crpss.pdf
61  * \author Zoltan Csaba Marton, Radu B. Rusu, Alexandru E. Ichim, Suat Gedikli
62  * \ingroup surface
63  */
64  template <typename PointInT, typename PointOutT>
65  class MovingLeastSquares: public CloudSurfaceProcessing<PointInT, PointOutT>
66  {
67  public:
68  typedef boost::shared_ptr<MovingLeastSquares<PointInT, PointOutT> > Ptr;
69  typedef boost::shared_ptr<const MovingLeastSquares<PointInT, PointOutT> > ConstPtr;
70 
76 
81 
85 
89 
90  typedef boost::function<int (int, double, std::vector<int> &, std::vector<float> &)> SearchMethod;
91 
93 
94  /** \brief Empty constructor. */
95  MovingLeastSquares () : CloudSurfaceProcessing<PointInT, PointOutT> (),
96  normals_ (),
97  distinct_cloud_ (),
98  search_method_ (),
99  tree_ (),
100  order_ (2),
101  polynomial_fit_ (true),
102  search_radius_ (0.0),
103  sqr_gauss_param_ (0.0),
104  compute_normals_ (false),
106  upsampling_radius_ (0.0),
107  upsampling_step_ (0.0),
109  mls_results_ (),
110  voxel_size_ (1.0),
112  nr_coeff_ (),
114  rng_alg_ (),
115  rng_uniform_distribution_ ()
116  {};
117 
118  /** \brief Empty destructor */
119  virtual ~MovingLeastSquares () {}
120 
121 
122  /** \brief Set whether the algorithm should also store the normals computed
123  * \note This is optional, but need a proper output cloud type
124  */
125  inline void
126  setComputeNormals (bool compute_normals) { compute_normals_ = compute_normals; }
127 
128  /** \brief Provide a pointer to the search object.
129  * \param[in] tree a pointer to the spatial search object.
130  */
131  inline void
133  {
134  tree_ = tree;
135  // Declare the search locator definition
136  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;
137  search_method_ = boost::bind (radiusSearch, boost::ref (tree_), _1, _2, _3, _4, 0);
138  }
139 
140  /** \brief Get a pointer to the search method used. */
141  inline KdTreePtr
142  getSearchMethod () { return (tree_); }
143 
144  /** \brief Set the order of the polynomial to be fit.
145  * \param[in] order the order of the polynomial
146  */
147  inline void
148  setPolynomialOrder (int order) { order_ = order; }
149 
150  /** \brief Get the order of the polynomial to be fit. */
151  inline int
152  getPolynomialOrder () { return (order_); }
153 
154  /** \brief Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimation.
155  * \param[in] polynomial_fit set to true for polynomial fit
156  */
157  inline void
158  setPolynomialFit (bool polynomial_fit) { polynomial_fit_ = polynomial_fit; }
159 
160  /** \brief Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial). */
161  inline bool
163 
164  /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting.
165  * \param[in] radius the sphere radius that is to contain all k-nearest neighbors
166  * \note Calling this method resets the squared Gaussian parameter to radius * radius !
167  */
168  inline void
170 
171  /** \brief Get the sphere radius used for determining the k-nearest neighbors. */
172  inline double
174 
175  /** \brief Set the parameter used for distance based weighting of neighbors (the square of the search radius works
176  * best in general).
177  * \param[in] sqr_gauss_param the squared Gaussian parameter
178  */
179  inline void
180  setSqrGaussParam (double sqr_gauss_param) { sqr_gauss_param_ = sqr_gauss_param; }
181 
182  /** \brief Get the parameter for distance based weighting of neighbors. */
183  inline double
184  getSqrGaussParam () const { return (sqr_gauss_param_); }
185 
186  /** \brief Set the upsampling method to be used
187  * \note Options are: * NONE - no upsampling will be done, only the input points will be projected to their own
188  * MLS surfaces
189  * * DISTINCT_CLOUD - will project the points of the distinct cloud to the closest point on
190  * the MLS surface
191  * * SAMPLE_LOCAL_PLANE - the local plane of each input point will be sampled in a circular
192  * fashion using the \ref upsampling_radius_ and the \ref upsampling_step_
193  * parameters
194  * * RANDOM_UNIFORM_DENSITY - the local plane of each input point will be sampled using an
195  * uniform random distribution such that the density of points is
196  * constant throughout the cloud - given by the \ref \ref desired_num_points_in_radius_
197  * parameter
198  * * VOXEL_GRID_DILATION - the input cloud will be inserted into a voxel grid with voxels of
199  * size \ref voxel_size_; this voxel grid will be dilated \ref dilation_iteration_num_
200  * times and the resulting points will be projected to the MLS surface
201  * of the closest point in the input cloud; the result is a point cloud
202  * with filled holes and a constant point density
203  */
204  inline void
206 
207  /** \brief Set the distinct cloud used for the DISTINCT_CLOUD upsampling method. */
208  inline void
209  setDistinctCloud (PointCloudInConstPtr distinct_cloud) { distinct_cloud_ = distinct_cloud; }
210 
211  /** \brief Get the distinct cloud used for the DISTINCT_CLOUD upsampling method. */
212  inline PointCloudInConstPtr
214 
215 
216  /** \brief Set the radius of the circle in the local point plane that will be sampled
217  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
218  * \param[in] radius the radius of the circle
219  */
220  inline void
221  setUpsamplingRadius (double radius) { upsampling_radius_ = radius; }
222 
223  /** \brief Get the radius of the circle in the local point plane that will be sampled
224  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
225  */
226  inline double
228 
229  /** \brief Set the step size for the local plane sampling
230  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
231  * \param[in] step_size the step size
232  */
233  inline void
234  setUpsamplingStepSize (double step_size) { upsampling_step_ = step_size; }
235 
236 
237  /** \brief Get the step size for the local plane sampling
238  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
239  */
240  inline double
242 
243  /** \brief Set the parameter that specifies the desired number of points within the search radius
244  * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
245  * \param[in] desired_num_points_in_radius the desired number of points in the output cloud in a sphere of
246  * radius \ref search_radius_ around each point
247  */
248  inline void
249  setPointDensity (int desired_num_points_in_radius) { desired_num_points_in_radius_ = desired_num_points_in_radius; }
250 
251 
252  /** \brief Get the parameter that specifies the desired number of points within the search radius
253  * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
254  */
255  inline int
257 
258  /** \brief Set the voxel size for the voxel grid
259  * \note Used only in the VOXEL_GRID_DILATION upsampling method
260  * \param[in] voxel_size the edge length of a cubic voxel in the voxel grid
261  */
262  inline void
263  setDilationVoxelSize (float voxel_size) { voxel_size_ = voxel_size; }
264 
265 
266  /** \brief Get the voxel size for the voxel grid
267  * \note Used only in the VOXEL_GRID_DILATION upsampling method
268  */
269  inline float
271 
272  /** \brief Set the number of dilation steps of the voxel grid
273  * \note Used only in the VOXEL_GRID_DILATION upsampling method
274  * \param[in] iterations the number of dilation iterations
275  */
276  inline void
277  setDilationIterations (int iterations) { dilation_iteration_num_ = iterations; }
278 
279  /** \brief Get the number of dilation steps of the voxel grid
280  * \note Used only in the VOXEL_GRID_DILATION upsampling method
281  */
282  inline int
284 
285  /** \brief Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()>
286  * \param[out] output the resultant reconstructed surface model
287  */
288  void
289  process (PointCloudOut &output);
290 
291 
292  /** \brief Get the set of indices with each point in output having the
293  * corresponding point in input */
294  inline PointIndicesPtr
296 
297  protected:
298  /** \brief The point cloud that will hold the estimated normals, if set. */
300 
301  /** \brief The distinct point cloud that will be projected to the MLS surface. */
303 
304  /** \brief The search method template for indices. */
306 
307  /** \brief A pointer to the spatial search object. */
309 
310  /** \brief The order of the polynomial to be fit. */
311  int order_;
312 
313  /** True if the surface and normal be approximated using a polynomial, false if tangent estimation is sufficient. */
315 
316  /** \brief The nearest neighbors search radius for each point. */
318 
319  /** \brief Parameter for distance based weighting of neighbors (search_radius_ * search_radius_ works fine) */
321 
322  /** \brief Parameter that specifies whether the normals should be computed for the input cloud or not */
324 
325  /** \brief Parameter that specifies the upsampling method to be used */
327 
328  /** \brief Radius of the circle in the local point plane that will be sampled
329  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
330  */
332 
333  /** \brief Step size for the local plane sampling
334  * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
335  */
337 
338  /** \brief Parameter that specifies the desired number of points within the search radius
339  * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
340  */
342 
343 
344  /** \brief Data structure used to store the results of the MLS fitting
345  * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling
346  */
347  struct MLSResult
348  {
349  MLSResult () : mean (), plane_normal (), u_axis (), v_axis (), c_vec (), num_neighbors (), curvature (), valid (false) {}
350 
351  MLSResult (const Eigen::Vector3d &a_mean,
352  const Eigen::Vector3d &a_plane_normal,
353  const Eigen::Vector3d &a_u,
354  const Eigen::Vector3d &a_v,
355  const Eigen::VectorXd a_c_vec,
356  const int a_num_neighbors,
357  const float &a_curvature);
358 
359  Eigen::Vector3d mean, plane_normal, u_axis, v_axis;
360  Eigen::VectorXd c_vec;
362  float curvature;
363  bool valid;
364  };
365 
366  /** \brief Stores the MLS result for each point in the input cloud
367  * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling
368  */
369  std::vector<MLSResult> mls_results_;
370 
371 
372  /** \brief A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling
373  * \note Used only in the case of VOXEL_GRID_DILATION upsampling
374  */
376  {
377  public:
378  struct Leaf { Leaf () : valid (true) {} bool valid; };
379 
381  IndicesPtr &indices,
382  float voxel_size);
383 
384  void
385  dilate ();
386 
387  inline void
388  getIndexIn1D (const Eigen::Vector3i &index, uint64_t &index_1d) const
389  {
390  index_1d = index[0] * data_size_ * data_size_ +
391  index[1] * data_size_ + index[2];
392  }
393 
394  inline void
395  getIndexIn3D (uint64_t index_1d, Eigen::Vector3i& index_3d) const
396  {
397  index_3d[0] = static_cast<Eigen::Vector3i::Scalar> (index_1d / (data_size_ * data_size_));
398  index_1d -= index_3d[0] * data_size_ * data_size_;
399  index_3d[1] = static_cast<Eigen::Vector3i::Scalar> (index_1d / data_size_);
400  index_1d -= index_3d[1] * data_size_;
401  index_3d[2] = static_cast<Eigen::Vector3i::Scalar> (index_1d);
402  }
403 
404  inline void
405  getCellIndex (const Eigen::Vector3f &p, Eigen::Vector3i& index) const
406  {
407  for (int i = 0; i < 3; ++i)
408  index[i] = static_cast<Eigen::Vector3i::Scalar> ((p[i] - bounding_min_(i)) / voxel_size_);
409  }
410 
411  inline void
412  getPosition (const uint64_t &index_1d, Eigen::Vector3f &point) const
413  {
414  Eigen::Vector3i index_3d;
415  getIndexIn3D (index_1d, index_3d);
416  for (int i = 0; i < 3; ++i)
417  point[i] = static_cast<Eigen::Vector3f::Scalar> (index_3d[i]) * voxel_size_ + bounding_min_[i];
418  }
419 
420  typedef std::map<uint64_t, Leaf> HashMap;
422  Eigen::Vector4f bounding_min_, bounding_max_;
423  uint64_t data_size_;
424  float voxel_size_;
425  };
426 
427 
428  /** \brief Voxel size for the VOXEL_GRID_DILATION upsampling method */
429  float voxel_size_;
430 
431  /** \brief Number of dilation steps for the VOXEL_GRID_DILATION upsampling method */
433 
434  /** \brief Number of coefficients, to be computed from the requested order.*/
436 
437  /** \brief Collects for each point in output the corrseponding point in the input. */
439 
440  /** \brief Search for the closest nearest neighbors of a given point using a radius search
441  * \param[in] index the index of the query point
442  * \param[out] indices the resultant vector of indices representing the k-nearest neighbors
443  * \param[out] sqr_distances the resultant squared distances from the query point to the k-nearest neighbors
444  */
445  inline int
446  searchForNeighbors (int index, std::vector<int> &indices, std::vector<float> &sqr_distances) const
447  {
448  return (search_method_ (index, search_radius_, indices, sqr_distances));
449  }
450 
451  /** \brief Smooth a given point and its neighborghood using Moving Least Squares.
452  * \param[in] index the inex of the query point in the \ref input cloud
453  * \param[in] nn_indices the set of nearest neighbors indices for \ref pt
454  * \param[in] nn_sqr_dists the set of nearest neighbors squared distances for \ref pt
455  * \param[out] projected_points the set of points projected points around the query point
456  * (in the case of upsampling method NONE, only the query point projected to its own fitted surface will be returned,
457  * in the case of the other upsampling methods, multiple points will be returned)
458  * \param[out] projected_points_normals the normals corresponding to the projected points
459  * \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input
460  * \param[out] mls_result stores the MLS result for each point in the input cloud
461  * (used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling)
462  */
463  void
464  computeMLSPointNormal (int index,
465  const std::vector<int> &nn_indices,
466  std::vector<float> &nn_sqr_dists,
467  PointCloudOut &projected_points,
468  NormalCloud &projected_points_normals,
469  PointIndices &corresponding_input_indices,
470  MLSResult &mls_result) const;
471 
472  /** \brief Fits a point (sample point) given in the local plane coordinates of an input point (query point) to
473  * the MLS surface of the input point
474  * \param[in] u_disp the u coordinate of the sample point in the local plane of the query point
475  * \param[in] v_disp the v coordinate of the sample point in the local plane of the query point
476  * \param[in] u the axis corresponding to the u-coordinates of the local plane of the query point
477  * \param[in] v the axis corresponding to the v-coordinates of the local plane of the query point
478  * \param[in] plane_normal the normal to the local plane of the query point
479  * \param[in] curvature the curvature of the surface at the query point
480  * \param[in] query_point the absolute 3D position of the query point
481  * \param[in] c_vec the coefficients of the polynomial fit on the MLS surface of the query point
482  * \param[in] num_neighbors the number of neighbors of the query point in the input cloud
483  * \param[out] result_point the absolute 3D position of the resulting projected point
484  * \param[out] result_normal the normal of the resulting projected point
485  */
486  void
487  projectPointToMLSSurface (float &u_disp, float &v_disp,
488  Eigen::Vector3d &u_axis, Eigen::Vector3d &v_axis,
489  Eigen::Vector3d &n_axis,
490  Eigen::Vector3d &mean,
491  float &curvature,
492  Eigen::VectorXd &c_vec,
493  int num_neighbors,
494  PointOutT &result_point,
495  pcl::Normal &result_normal) const;
496 
497  void
498  copyMissingFields (const PointInT &point_in,
499  PointOutT &point_out) const;
500 
501  /** \brief Abstract surface reconstruction method.
502  * \param[out] output the result of the reconstruction
503  */
504  virtual void performProcessing (PointCloudOut &output);
505 
506  /** \brief Perform upsampling for the distinct-cloud and voxel-grid methods
507  * \param[out] output the result of the reconstruction
508  */
509  void performUpsampling (PointCloudOut &output);
510 
511  private:
512  /** \brief Boost-based random number generator algorithm. */
513  boost::mt19937 rng_alg_;
514 
515  /** \brief Random number generator using an uniform distribution of floats
516  * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
517  */
518  boost::shared_ptr<boost::variate_generator<boost::mt19937&,
519  boost::uniform_real<float> >
520  > rng_uniform_distribution_;
521 
522  /** \brief Abstract class get name method. */
523  std::string getClassName () const { return ("MovingLeastSquares"); }
524 
525  public:
526  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
527  };
528 
529 #ifdef _OPENMP
530  /** \brief MovingLeastSquaresOMP is a parallelized version of MovingLeastSquares, using the OpenMP standard.
531  * \note Compared to MovingLeastSquares, an overhead is incurred in terms of runtime and memory usage.
532  * \note The upsampling methods DISTINCT_CLOUD and VOXEL_GRID_DILATION are not parallelized completely, i.e. parts of the algorithm run on a single thread only.
533  * \author Robert Huitl
534  * \ingroup surface
535  */
536  template <typename PointInT, typename PointOutT>
537  class MovingLeastSquaresOMP: public MovingLeastSquares<PointInT, PointOutT>
538  {
539  public:
540  typedef boost::shared_ptr<MovingLeastSquares<PointInT, PointOutT> > Ptr;
541  typedef boost::shared_ptr<const MovingLeastSquares<PointInT, PointOutT> > ConstPtr;
542 
545  using MovingLeastSquares<PointInT, PointOutT>::normals_;
546  using MovingLeastSquares<PointInT, PointOutT>::corresponding_input_indices_;
547  using MovingLeastSquares<PointInT, PointOutT>::nr_coeff_;
548  using MovingLeastSquares<PointInT, PointOutT>::order_;
549  using MovingLeastSquares<PointInT, PointOutT>::compute_normals_;
550 
551  typedef pcl::PointCloud<pcl::Normal> NormalCloud;
552  typedef pcl::PointCloud<pcl::Normal>::Ptr NormalCloudPtr;
553 
554  typedef pcl::PointCloud<PointOutT> PointCloudOut;
555  typedef typename PointCloudOut::Ptr PointCloudOutPtr;
556  typedef typename PointCloudOut::ConstPtr PointCloudOutConstPtr;
557 
558  /** \brief Constructor for parallelized Moving Least Squares
559  * \param threads the maximum number of hardware threads to use (0 sets the value to 1)
560  */
561  MovingLeastSquaresOMP (unsigned int threads = 0) : threads_ (threads)
562  {
563 
564  }
565 
566  /** \brief Set the maximum number of threads to use
567  * \param threads the maximum number of hardware threads to use (0 sets the value to 1)
568  */
569  inline void
570  setNumberOfThreads (unsigned int threads = 0)
571  {
572  threads_ = threads;
573  }
574 
575  protected:
576  /** \brief Abstract surface reconstruction method.
577  * \param[out] output the result of the reconstruction
578  */
579  virtual void performProcessing (PointCloudOut &output);
580 
581  /** \brief The maximum number of threads the scheduler should use. */
582  unsigned int threads_;
583  };
584 #endif
585 }
586 
587 #ifdef PCL_NO_PRECOMPILE
588 #include <pcl/surface/impl/mls.hpp>
589 #endif
590 
591 #endif /* #ifndef PCL_MLS_H_ */