Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
55namespace 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 ub 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 neighborghood using Moving Least Squares.
210 * \param[in] index the index of the query point in the input cloud
211 * \param[in] nn_indices the set of nearest neighbors indices for pt
212 * \param[in] search_radius the search radius used to find nearest neighbors for pt
213 * \param[in] polynomial_order the order of the polynomial to fit to the nearest neighbors
214 * \param[in] weight_func defines the weight function for the polynomial fit
215 */
216 template <typename PointT> void
218 pcl::index_t index,
219 const pcl::Indices &nn_indices,
220 double search_radius,
221 int polynomial_order = 2,
222 std::function<double(const double)> weight_func = {});
223
224 Eigen::Vector3d query_point; /**< \brief The query point about which the mls surface was generated */
225 Eigen::Vector3d mean; /**< \brief The mean point of all the neighbors. */
226 Eigen::Vector3d plane_normal; /**< \brief The normal of the local plane of the query point. */
227 Eigen::Vector3d u_axis; /**< \brief The axis corresponding to the u-coordinates of the local plane of the query point. */
228 Eigen::Vector3d v_axis; /**< \brief The axis corresponding to the v-coordinates of the local plane of the query point. */
229 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 */
230 int num_neighbors; /**< \brief The number of neighbors used to create the mls surface. */
231 float curvature; /**< \brief The curvature at the query point. */
232 int order; /**< \brief The order of the polynomial. If order > 1 then use polynomial fit */
233 bool valid; /**< \brief If True, the mls results data is valid, otherwise False. */
235 private:
236 /**
237 * \brief The default weight function used when fitting a polynomial surface
238 * \param sq_dist the squared distance from a point to origin of the mls frame
239 * \param sq_mls_radius the squraed mls search radius used
240 * \return The weight for a point at squared distance from the origin of the mls frame
241 */
242 inline
243 double computeMLSWeight (const double sq_dist, const double sq_mls_radius) { return (std::exp (-sq_dist / sq_mls_radius)); }
244
245 };
246
247 /** \brief MovingLeastSquares represent an implementation of the MLS (Moving Least Squares) algorithm
248 * for data smoothing and improved normal estimation. It also contains methods for upsampling the
249 * resulting cloud based on the parametric fit.
250 * Reference paper: "Computing and Rendering Point Set Surfaces" by Marc Alexa, Johannes Behr,
251 * Daniel Cohen-Or, Shachar Fleishman, David Levin and Claudio T. Silva
252 * www.sci.utah.edu/~shachar/Publications/crpss.pdf
253 * \note There is a parallelized version of the processing step, using the OpenMP standard.
254 * Compared to the standard version, an overhead is incurred in terms of runtime and memory usage.
255 * The upsampling methods DISTINCT_CLOUD and VOXEL_GRID_DILATION are not parallelized completely,
256 * i.e. parts of the algorithm run on a single thread only.
257 * \author Zoltan Csaba Marton, Radu B. Rusu, Alexandru E. Ichim, Suat Gedikli, Robert Huitl
258 * \ingroup surface
259 */
260 template <typename PointInT, typename PointOutT>
261 class MovingLeastSquares : public CloudSurfaceProcessing<PointInT, PointOutT>
262 {
263 public:
264 typedef shared_ptr<MovingLeastSquares<PointInT, PointOutT> > Ptr;
265 typedef shared_ptr<const MovingLeastSquares<PointInT, PointOutT> > ConstPtr;
266
267 using PCLBase<PointInT>::input_;
268 using PCLBase<PointInT>::indices_;
269 using PCLBase<PointInT>::fake_indices_;
270 using PCLBase<PointInT>::initCompute;
271 using PCLBase<PointInT>::deinitCompute;
272
274 using KdTreePtr = typename KdTree::Ptr;
277
281
285
286 using SearchMethod = std::function<int (pcl::index_t, double, pcl::Indices &, std::vector<float> &)>;
287
289 {
290 NONE, /**< \brief No upsampling will be done, only the input points will be projected
291 to their own MLS surfaces. */
292 DISTINCT_CLOUD, /**< \brief Project the points of the distinct cloud to the MLS surface. */
293 SAMPLE_LOCAL_PLANE, /**< \brief The local plane of each input point will be sampled in a circular fashion
294 using the \ref upsampling_radius_ and the \ref upsampling_step_ parameters. */
295 RANDOM_UNIFORM_DENSITY, /**< \brief The local plane of each input point will be sampled using an uniform random
296 distribution such that the density of points is constant throughout the
297 cloud - given by the \ref desired_num_points_in_radius_ parameter. */
298 VOXEL_GRID_DILATION /**< \brief The input cloud will be inserted into a voxel grid with voxels of
299 size \ref voxel_size_; this voxel grid will be dilated \ref dilation_iteration_num_
300 times and the resulting points will be projected to the MLS surface
301 of the closest point in the input cloud; the result is a point cloud
302 with filled holes and a constant point density. */
303 };
304
305 /** \brief Empty constructor. */
306 MovingLeastSquares () : CloudSurfaceProcessing<PointInT, PointOutT> (),
308 tree_ (),
309 order_ (2),
310 search_radius_ (0.0),
311 sqr_gauss_param_ (0.0),
312 compute_normals_ (false),
314 upsampling_radius_ (0.0),
315 upsampling_step_ (0.0),
317 cache_mls_results_ (true),
319 threads_ (1),
320 voxel_size_ (1.0),
322 nr_coeff_ (),
323 rng_uniform_distribution_ ()
324 {};
325
326 /** \brief Empty destructor */
328
329
330 /** \brief Set whether the algorithm should also store the normals computed
331 * \note This is optional, but need a proper output cloud type
332 */
333 inline void
334 setComputeNormals (bool compute_normals) { compute_normals_ = compute_normals; }
335
336 /** \brief Provide a pointer to the search object.
337 * \param[in] tree a pointer to the spatial search object.
338 */
339 inline void
341 {
342 tree_ = tree;
343 // Declare the search locator definition
344 search_method_ = [this] (pcl::index_t index, double radius, pcl::Indices& k_indices, std::vector<float>& k_sqr_distances)
345 {
346 return tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, 0);
347 };
348 }
349
350 /** \brief Get a pointer to the search method used. */
351 inline KdTreePtr
352 getSearchMethod () const { return (tree_); }
353
354 /** \brief Set the order of the polynomial to be fit.
355 * \param[in] order the order of the polynomial
356 * \note Setting order > 1 indicates using a polynomial fit.
357 */
358 inline void
359 setPolynomialOrder (int order) { order_ = order; }
360
361 /** \brief Get the order of the polynomial to be fit. */
362 inline int
363 getPolynomialOrder () const { return (order_); }
364
365 /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting.
366 * \param[in] radius the sphere radius that is to contain all k-nearest neighbors
367 * \note Calling this method resets the squared Gaussian parameter to radius * radius !
368 */
369 inline void
371
372 /** \brief Get the sphere radius used for determining the k-nearest neighbors. */
373 inline double
374 getSearchRadius () const { return (search_radius_); }
375
376 /** \brief Set the parameter used for distance based weighting of neighbors (the square of the search radius works
377 * best in general).
378 * \param[in] sqr_gauss_param the squared Gaussian parameter
379 */
380 inline void
381 setSqrGaussParam (double sqr_gauss_param) { sqr_gauss_param_ = sqr_gauss_param; }
382
383 /** \brief Get the parameter for distance based weighting of neighbors. */
384 inline double
385 getSqrGaussParam () const { return (sqr_gauss_param_); }
386
387 /** \brief Set the upsampling method to be used
388 * \param method
389 */
390 inline void
392
393 /** \brief Set the distinct cloud used for the DISTINCT_CLOUD upsampling method. */
394 inline void
395 setDistinctCloud (PointCloudInConstPtr distinct_cloud) { distinct_cloud_ = distinct_cloud; }
396
397 /** \brief Get the distinct cloud used for the DISTINCT_CLOUD upsampling method. */
399 getDistinctCloud () const { return (distinct_cloud_); }
400
401
402 /** \brief Set the radius of the circle in the local point plane that will be sampled
403 * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
404 * \param[in] radius the radius of the circle
405 */
406 inline void
407 setUpsamplingRadius (double radius) { upsampling_radius_ = radius; }
408
409 /** \brief Get the radius of the circle in the local point plane that will be sampled
410 * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
411 */
412 inline double
414
415 /** \brief Set the step size for the local plane sampling
416 * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
417 * \param[in] step_size the step size
418 */
419 inline void
420 setUpsamplingStepSize (double step_size) { upsampling_step_ = step_size; }
421
422
423 /** \brief Get the step size for the local plane sampling
424 * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
425 */
426 inline double
428
429 /** \brief Set the parameter that specifies the desired number of points within the search radius
430 * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
431 * \param[in] desired_num_points_in_radius the desired number of points in the output cloud in a sphere of
432 * radius \ref search_radius_ around each point
433 */
434 inline void
435 setPointDensity (int desired_num_points_in_radius) { desired_num_points_in_radius_ = desired_num_points_in_radius; }
436
437
438 /** \brief Get the parameter that specifies the desired number of points within the search radius
439 * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
440 */
441 inline int
443
444 /** \brief Set the voxel size for the voxel grid
445 * \note Used only in the VOXEL_GRID_DILATION upsampling method
446 * \param[in] voxel_size the edge length of a cubic voxel in the voxel grid
447 */
448 inline void
449 setDilationVoxelSize (float voxel_size) { voxel_size_ = voxel_size; }
450
451
452 /** \brief Get the voxel size for the voxel grid
453 * \note Used only in the VOXEL_GRID_DILATION upsampling method
454 */
455 inline float
456 getDilationVoxelSize () const { return (voxel_size_); }
457
458 /** \brief Set the number of dilation steps of the voxel grid
459 * \note Used only in the VOXEL_GRID_DILATION upsampling method
460 * \param[in] iterations the number of dilation iterations
461 */
462 inline void
463 setDilationIterations (int iterations) { dilation_iteration_num_ = iterations; }
464
465 /** \brief Get the number of dilation steps of the voxel grid
466 * \note Used only in the VOXEL_GRID_DILATION upsampling method
467 */
468 inline int
470
471 /** \brief Set whether the mls results should be stored for each point in the input cloud
472 * \param[in] cache_mls_results True if the mls results should be stored, otherwise false.
473 * \note The cache_mls_results_ is forced to be true when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.
474 * \note If memory consumption is a concern, then set it to false when not using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.
475 */
476 inline void
477 setCacheMLSResults (bool cache_mls_results) { cache_mls_results_ = cache_mls_results; }
478
479 /** \brief Get the cache_mls_results_ value (True if the mls results should be stored, otherwise false). */
480 inline bool
482
483 /** \brief Set the method to be used when projection the point on to the MLS surface.
484 * \param method
485 * \note This is only used when polynomial fit is enabled.
486 */
487 inline void
489
490
491 /** \brief Get the current projection method being used. */
494
495 /** \brief Get the MLSResults for input cloud
496 * \note The results are only stored if setCacheMLSResults(true) was called or when using the upsampling method DISTINCT_CLOUD or VOXEL_GRID_DILATION.
497 * \note This vector is aligned with the input cloud indices, so use getCorrespondingIndices to get the correct results when using output cloud indices.
498 */
499 inline const std::vector<MLSResult>&
500 getMLSResults () const { return (mls_results_); }
501
502 /** \brief Set the maximum number of threads to use
503 * \param threads the maximum number of hardware threads to use (0 sets the value to 1)
504 */
505 inline void
506 setNumberOfThreads (unsigned int threads = 1)
507 {
508 threads_ = threads;
509 }
510
511 /** \brief Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()>
512 * \param[out] output the resultant reconstructed surface model
513 */
514 void
515 process (PointCloudOut &output) override;
516
517
518 /** \brief Get the set of indices with each point in output having the
519 * corresponding point in input */
520 inline PointIndicesPtr
522
523 protected:
524 /** \brief The point cloud that will hold the estimated normals, if set. */
526
527 /** \brief The distinct point cloud that will be projected to the MLS surface. */
529
530 /** \brief The search method template for indices. */
532
533 /** \brief A pointer to the spatial search object. */
535
536 /** \brief The order of the polynomial to be fit. */
538
539 /** \brief The nearest neighbors search radius for each point. */
541
542 /** \brief Parameter for distance based weighting of neighbors (search_radius_ * search_radius_ works fine) */
544
545 /** \brief Parameter that specifies whether the normals should be computed for the input cloud or not */
547
548 /** \brief Parameter that specifies the upsampling method to be used */
550
551 /** \brief Radius of the circle in the local point plane that will be sampled
552 * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
553 */
555
556 /** \brief Step size for the local plane sampling
557 * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
558 */
560
561 /** \brief Parameter that specifies the desired number of points within the search radius
562 * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
563 */
565
566 /** \brief True if the mls results for the input cloud should be stored
567 * \note This is forced to be true when using upsampling methods VOXEL_GRID_DILATION or DISTINCT_CLOUD.
568 */
570
571 /** \brief Stores the MLS result for each point in the input cloud
572 * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling
573 */
574 std::vector<MLSResult> mls_results_;
575
576 /** \brief Parameter that specifies the projection method to be used. */
578
579 /** \brief The maximum number of threads the scheduler should use. */
580 unsigned int threads_;
581
582
583 /** \brief A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling
584 * \note Used only in the case of VOXEL_GRID_DILATION upsampling
585 */
587 {
588 public:
589 struct Leaf { Leaf () : valid (true) {} bool valid; };
590
592 IndicesPtr &indices,
593 float voxel_size);
594
595 void
596 dilate ();
597
598 inline void
599 getIndexIn1D (const Eigen::Vector3i &index, std::uint64_t &index_1d) const
600 {
601 index_1d = index[0] * data_size_ * data_size_ +
602 index[1] * data_size_ + index[2];
603 }
604
605 inline void
606 getIndexIn3D (std::uint64_t index_1d, Eigen::Vector3i& index_3d) const
607 {
608 index_3d[0] = static_cast<Eigen::Vector3i::Scalar> (index_1d / (data_size_ * data_size_));
609 index_1d -= index_3d[0] * data_size_ * data_size_;
610 index_3d[1] = static_cast<Eigen::Vector3i::Scalar> (index_1d / data_size_);
611 index_1d -= index_3d[1] * data_size_;
612 index_3d[2] = static_cast<Eigen::Vector3i::Scalar> (index_1d);
613 }
614
615 inline void
616 getCellIndex (const Eigen::Vector3f &p, Eigen::Vector3i& index) const
617 {
618 for (int i = 0; i < 3; ++i)
619 index[i] = static_cast<Eigen::Vector3i::Scalar> ((p[i] - bounding_min_ (i)) / voxel_size_);
620 }
621
622 inline void
623 getPosition (const std::uint64_t &index_1d, Eigen::Vector3f &point) const
624 {
625 Eigen::Vector3i index_3d;
626 getIndexIn3D (index_1d, index_3d);
627 for (int i = 0; i < 3; ++i)
628 point[i] = static_cast<Eigen::Vector3f::Scalar> (index_3d[i]) * voxel_size_ + bounding_min_[i];
629 }
630
631 typedef std::map<std::uint64_t, Leaf> HashMap;
633 Eigen::Vector4f bounding_min_, bounding_max_;
634 std::uint64_t data_size_;
637 };
638
639
640 /** \brief Voxel size for the VOXEL_GRID_DILATION upsampling method */
642
643 /** \brief Number of dilation steps for the VOXEL_GRID_DILATION upsampling method */
645
646 /** \brief Number of coefficients, to be computed from the requested order.*/
648
649 /** \brief Collects for each point in output the corrseponding point in the input. */
651
652 /** \brief Search for the nearest neighbors of a given point using a radius search
653 * \param[in] index the index of the query point
654 * \param[out] indices the resultant vector of indices representing the neighbors within search_radius_
655 * \param[out] sqr_distances the resultant squared distances from the query point to the neighbors within search_radius_
656 */
657 inline int
658 searchForNeighbors (pcl::index_t index, pcl::Indices &indices, std::vector<float> &sqr_distances) const
659 {
660 return (search_method_ (index, search_radius_, indices, sqr_distances));
661 }
662
663 /** \brief Smooth a given point and its neighborghood using Moving Least Squares.
664 * \param[in] index the index of the query point in the input cloud
665 * \param[in] nn_indices the set of nearest neighbors indices for pt
666 * \param[out] projected_points the set of projected points around the query point
667 * (in the case of upsampling method NONE, only the query point projected to its own fitted surface will be returned,
668 * in the case of the other upsampling methods, multiple points will be returned)
669 * \param[out] projected_points_normals the normals corresponding to the projected points
670 * \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input
671 * \param[out] mls_result stores the MLS result for each point in the input cloud
672 * (used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling)
673 */
674 void
676 const pcl::Indices &nn_indices,
677 PointCloudOut &projected_points,
678 NormalCloud &projected_points_normals,
679 PointIndices &corresponding_input_indices,
680 MLSResult &mls_result) const;
681
682
683 /** \brief This is a helper function for adding projected points
684 * \param[in] index the index of the query point in the input cloud
685 * \param[in] point the projected point to be added
686 * \param[in] normal the projected point's normal to be added
687 * \param[in] curvature the projected point's curvature
688 * \param[out] projected_points the set of projected points around the query point
689 * \param[out] projected_points_normals the normals corresponding to the projected points
690 * \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input
691 */
692 void
694 const Eigen::Vector3d &point,
695 const Eigen::Vector3d &normal,
696 double curvature,
697 PointCloudOut &projected_points,
698 NormalCloud &projected_points_normals,
699 PointIndices &corresponding_input_indices) const;
700
701
702 void
703 copyMissingFields (const PointInT &point_in,
704 PointOutT &point_out) const;
705
706 /** \brief Abstract surface reconstruction method.
707 * \param[out] output the result of the reconstruction
708 */
709 void
710 performProcessing (PointCloudOut &output) override;
711
712 /** \brief Perform upsampling for the distinct-cloud and voxel-grid methods
713 * \param[out] output the result of the reconstruction
714 */
715 void
717
718 private:
719 /** \brief Random number generator algorithm. */
720 mutable std::mt19937 rng_;
721
722 /** \brief Random number generator using an uniform distribution of floats
723 * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
724 */
725 std::unique_ptr<std::uniform_real_distribution<>> rng_uniform_distribution_;
726
727 /** \brief Abstract class get name method. */
728 std::string
729 getClassName () const { return ("MovingLeastSquares"); }
730 };
731}
732
733#ifdef PCL_NO_PRECOMPILE
734#include <pcl/surface/impl/mls.hpp>
735#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:587
MLSVoxelGrid(PointCloudInConstPtr &cloud, IndicesPtr &indices, float voxel_size)
Definition mls.hpp:806
void getPosition(const std::uint64_t &index_1d, Eigen::Vector3f &point) const
Definition mls.h:623
void getIndexIn1D(const Eigen::Vector3i &index, std::uint64_t &index_1d) const
Definition mls.h:599
std::map< std::uint64_t, Leaf > HashMap
Definition mls.h:631
void getCellIndex(const Eigen::Vector3f &p, Eigen::Vector3i &index) const
Definition mls.h:616
void getIndexIn3D(std::uint64_t index_1d, Eigen::Vector3i &index_3d) const
Definition mls.h:606
MovingLeastSquares represent an implementation of the MLS (Moving Least Squares) algorithm for data s...
Definition mls.h:262
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:381
void setDilationIterations(int iterations)
Set the number of dilation steps of the voxel grid.
Definition mls.h:463
bool getCacheMLSResults() const
Get the cache_mls_results_ value (True if the mls results should be stored, otherwise false).
Definition mls.h:481
double getSqrGaussParam() const
Get the parameter for distance based weighting of neighbors.
Definition mls.h:385
unsigned int threads_
The maximum number of threads the scheduler should use.
Definition mls.h:580
void performUpsampling(PointCloudOut &output)
Perform upsampling for the distinct-cloud and voxel-grid methods.
Definition mls.hpp:370
~MovingLeastSquares()
Empty destructor.
Definition mls.h:327
typename PointCloudIn::Ptr PointCloudInPtr
Definition mls.h:283
int order_
The order of the polynomial to be fit.
Definition mls.h:537
double getSearchRadius() const
Get the sphere radius used for determining the k-nearest neighbors.
Definition mls.h:374
typename KdTree::Ptr KdTreePtr
Definition mls.h:274
MLSResult::ProjectionMethod projection_method_
Parameter that specifies the projection method to be used.
Definition mls.h:577
typename PointCloudOut::Ptr PointCloudOutPtr
Definition mls.h:279
KdTreePtr getSearchMethod() const
Get a pointer to the search method used.
Definition mls.h:352
void setPolynomialOrder(int order)
Set the order of the polynomial to be fit.
Definition mls.h:359
int getPolynomialOrder() const
Get the order of the polynomial to be fit.
Definition mls.h:363
double getUpsamplingRadius() const
Get the radius of the circle in the local point plane that will be sampled.
Definition mls.h:413
double search_radius_
The nearest neighbors search radius for each point.
Definition mls.h:540
MovingLeastSquares()
Empty constructor.
Definition mls.h:306
pcl::PointCloud< PointOutT > PointCloudOut
Definition mls.h:278
double sqr_gauss_param_
Parameter for distance based weighting of neighbors (search_radius_ * search_radius_ works fine)
Definition mls.h:543
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition mls.h:284
int getPointDensity() const
Get the parameter that specifies the desired number of points within the search radius.
Definition mls.h:442
std::function< int(pcl::index_t, double, pcl::Indices &, std::vector< float > &)> SearchMethod
Definition mls.h:286
float getDilationVoxelSize() const
Get the voxel size for the voxel grid.
Definition mls.h:456
KdTreePtr tree_
A pointer to the spatial search object.
Definition mls.h:534
void copyMissingFields(const PointInT &point_in, PointOutT &point_out) const
Definition mls.hpp:861
void setComputeNormals(bool compute_normals)
Set whether the algorithm should also store the normals computed.
Definition mls.h:334
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:435
MLSResult::ProjectionMethod getProjectionMethod() const
Get the current projection method being used.
Definition mls.h:493
double getUpsamplingStepSize() const
Get the step size for the local plane sampling.
Definition mls.h:427
int getDilationIterations() const
Get the number of dilation steps of the voxel grid.
Definition mls.h:469
double upsampling_step_
Step size for the local plane sampling.
Definition mls.h:559
shared_ptr< const MovingLeastSquares< PointInT, PointOutT > > ConstPtr
Definition mls.h:265
NormalCloud::Ptr NormalCloudPtr
Definition mls.h:276
void setUpsamplingRadius(double radius)
Set the radius of the circle in the local point plane that will be sampled.
Definition mls.h:407
NormalCloudPtr normals_
The point cloud that will hold the estimated normals, if set.
Definition mls.h:525
void setDistinctCloud(PointCloudInConstPtr distinct_cloud)
Set the distinct cloud used for the DISTINCT_CLOUD upsampling method.
Definition mls.h:395
void setDilationVoxelSize(float voxel_size)
Set the voxel size for the voxel grid.
Definition mls.h:449
UpsamplingMethod upsample_method_
Parameter that specifies the upsampling method to be used.
Definition mls.h:549
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:658
double upsampling_radius_
Radius of the circle in the local point plane that will be sampled.
Definition mls.h:554
@ RANDOM_UNIFORM_DENSITY
The local plane of each input point will be sampled using an uniform random distribution such that th...
Definition mls.h:295
@ SAMPLE_LOCAL_PLANE
The local plane of each input point will be sampled in a circular fashion using the upsampling_radius...
Definition mls.h:293
@ 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:298
@ NONE
No upsampling will be done, only the input points will be projected to their own MLS surfaces.
Definition mls.h:290
@ DISTINCT_CLOUD
Project the points of the distinct cloud to the MLS surface.
Definition mls.h:292
PointCloudInConstPtr getDistinctCloud() const
Get the distinct cloud used for the DISTINCT_CLOUD upsampling method.
Definition mls.h:399
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition mls.h:340
int desired_num_points_in_radius_
Parameter that specifies the desired number of points within the search radius.
Definition mls.h:564
pcl::PointCloud< pcl::Normal > NormalCloud
Definition mls.h:275
void setUpsamplingMethod(UpsamplingMethod method)
Set the upsampling method to be used.
Definition mls.h:391
void setUpsamplingStepSize(double step_size)
Set the step size for the local plane sampling.
Definition mls.h:420
PointIndicesPtr corresponding_input_indices_
Collects for each point in output the corrseponding point in the input.
Definition mls.h:650
void setCacheMLSResults(bool cache_mls_results)
Set whether the mls results should be stored for each point in the input cloud.
Definition mls.h:477
int nr_coeff_
Number of coefficients, to be computed from the requested order.
Definition mls.h:647
void setNumberOfThreads(unsigned int threads=1)
Set the maximum number of threads to use.
Definition mls.h:506
bool compute_normals_
Parameter that specifies whether the normals should be computed for the input cloud or not.
Definition mls.h:546
void performProcessing(PointCloudOut &output) override
Abstract surface reconstruction method.
Definition mls.hpp:284
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:174
const std::vector< MLSResult > & getMLSResults() const
Get the MLSResults for input cloud.
Definition mls.h:500
SearchMethod search_method_
The search method template for indices.
Definition mls.h:531
void process(PointCloudOut &output) override
Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()>
Definition mls.hpp:61
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:252
PointIndicesPtr getCorrespondingIndices() const
Get the set of indices with each point in output having the corresponding point in input.
Definition mls.h:521
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:370
typename PointCloudOut::ConstPtr PointCloudOutConstPtr
Definition mls.h:280
int dilation_iteration_num_
Number of dilation steps for the VOXEL_GRID_DILATION upsampling method.
Definition mls.h:644
void setProjectionMethod(MLSResult::ProjectionMethod method)
Set the method to be used when projection the point on to the MLS surface.
Definition mls.h:488
bool cache_mls_results_
True if the mls results for the input cloud should be stored.
Definition mls.h:569
shared_ptr< MovingLeastSquares< PointInT, PointOutT > > Ptr
Definition mls.h:264
std::vector< MLSResult > mls_results_
Stores the MLS result for each point in the input cloud.
Definition mls.h:574
float voxel_size_
Voxel size for the VOXEL_GRID_DILATION upsampling method.
Definition mls.h:641
PointCloudInConstPtr distinct_cloud_
The distinct point cloud that will be projected to the MLS surface.
Definition mls.h:528
PCL base class.
Definition pcl_base.h:70
PointCloudConstPtr input_
Definition pcl_base.h:147
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< PointCloud< PointT > > Ptr
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
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
PointIndices::Ptr PointIndicesPtr
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:637
MLSResult()
Definition mls.h:92
Eigen::Vector3d mean
The mean point of all the neighbors.
Definition mls.h:225
MLSProjectionResults projectPointOrthogonalToPolynomialSurface(const double u, const double v, const double w) const
Project a point orthogonal to the polynomial surface.
Definition mls.hpp:537
Eigen::Vector3d u_axis
The axis corresponding to the u-coordinates of the local plane of the query point.
Definition mls.h:227
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:226
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:228
int num_neighbors
The number of neighbors used to create the mls surface.
Definition mls.h:230
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:229
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 neighborghood using Moving Least Squares.
Definition mls.hpp:690
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:453
float curvature
The curvature at the query point.
Definition mls.h:231
PolynomialPartialDerivative getPolynomialPartialDerivative(const double u, const double v) const
Calculate the polynomial's first and second partial derivatives.
Definition mls.hpp:492
MLSProjectionResults projectPointSimpleToPolynomialSurface(const double u, const double v) const
Project a point along the MLS plane normal to the polynomial surface.
Definition mls.hpp:614
MLSProjectionResults projectPointToMLSPlane(const double u, const double v) const
Project a point onto the MLS plane.
Definition mls.hpp:602
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:470
Eigen::Vector3d query_point
The query point about which the mls surface was generated.
Definition mls.h:224
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:659
int order
The order of the polynomial.
Definition mls.h:232
bool valid
If True, the mls results data is valid, otherwise False.
Definition mls.h:233