From e8c036e47e68fe8a84d372129501f20ef9eb971a Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 28 Jul 2017 14:56:49 -0500 Subject: [PATCH 1/4] Add ability to cache mls results --- surface/include/pcl/surface/impl/mls.hpp | 26 ++++++---- surface/include/pcl/surface/mls.h | 64 ++++++++++++++++-------- 2 files changed, 59 insertions(+), 31 deletions(-) diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index f7df1399c91..a346befff92 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -116,24 +116,30 @@ pcl::MovingLeastSquares::process (PointCloudOut &output) float tmp = static_cast (search_radius_ / 2.0f); boost::uniform_real uniform_distrib (-tmp, tmp); rng_uniform_distribution_.reset (new boost::variate_generator > (rng_alg_, uniform_distrib)); - - mls_results_.resize (1); // Need to have a reference to a single dummy result. break; } case (VOXEL_GRID_DILATION): case (DISTINCT_CLOUD): { - mls_results_.resize (input_->size ()); + cache_mls_results_ = true; break; } default: { - mls_results_.resize (1); // Need to have a reference to a single dummy result. break; } } + if (cache_mls_results_) + { + mls_results_.resize (input_->size ()); + } + else + { + mls_results_.resize (1); // Need to have a reference to a single dummy result. + } + // Perform the actual surface reconstruction performProcessing (output); @@ -271,6 +277,11 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, P_weight_Pt.llt ().solveInPlace (c_vec); } + if (cache_mls_results_) + { + mls_result = MLSResult (point, plane_normal, u_axis, v_axis, c_vec, static_cast (nn_indices.size ()), curvature); + } + switch (upsample_method_) { case (NONE): @@ -396,9 +407,6 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, case (VOXEL_GRID_DILATION): case (DISTINCT_CLOUD): { - // Take all point pairs and sample space between them in a grid-fashion - // \note consider only point pairs with increasing indices - mls_result = MLSResult (point, plane_normal, u_axis, v_axis, c_vec, static_cast (nn_indices.size ()), curvature); break; } } @@ -494,7 +502,7 @@ pcl::MovingLeastSquares::performProcessing (PointCloudOut & // Get a plane approximating the local surface's tangent and project point onto it int index = (*indices_)[cp]; - if (upsample_method_ == VOXEL_GRID_DILATION || upsample_method_ == DISTINCT_CLOUD) + if (cache_mls_results_) mls_result_index = index; // otherwise we give it a dummy location. computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]); @@ -557,7 +565,7 @@ pcl::MovingLeastSquaresOMP::performProcessing (PointCloudOu int index = (*indices_)[cp]; size_t mls_result_index = 0; - if (upsample_method_ == VOXEL_GRID_DILATION || upsample_method_ == DISTINCT_CLOUD) + if (this->cache_mls_results_) mls_result_index = index; // otherwise we give it a dummy location. this->computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[mls_result_index]); diff --git a/surface/include/pcl/surface/mls.h b/surface/include/pcl/surface/mls.h index 8aeb9a2f363..658d067a119 100644 --- a/surface/include/pcl/surface/mls.h +++ b/surface/include/pcl/surface/mls.h @@ -91,6 +91,28 @@ namespace pcl enum UpsamplingMethod {NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION}; + /** \brief Data structure used to store the results of the MLS fitting + * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling + */ + struct MLSResult + { + MLSResult () : mean (), plane_normal (), u_axis (), v_axis (), c_vec (), num_neighbors (), curvature (), valid (false) {} + + MLSResult (const Eigen::Vector3d &a_mean, + const Eigen::Vector3d &a_plane_normal, + const Eigen::Vector3d &a_u, + const Eigen::Vector3d &a_v, + const Eigen::VectorXd a_c_vec, + const int a_num_neighbors, + const float &a_curvature); + + Eigen::Vector3d mean, plane_normal, u_axis, v_axis; + Eigen::VectorXd c_vec; + int num_neighbors; + float curvature; + bool valid; + }; + /** \brief Empty constructor. */ MovingLeastSquares () : CloudSurfaceProcessing (), normals_ (), @@ -106,6 +128,7 @@ namespace pcl upsampling_radius_ (0.0), upsampling_step_ (0.0), desired_num_points_in_radius_ (0), + cache_mls_results_ (false), mls_results_ (), voxel_size_ (1.0), dilation_iteration_num_ (0), @@ -283,6 +306,22 @@ namespace pcl inline int getDilationIterations () { return dilation_iteration_num_; } + /** \brief Set wether the mls results should be stored for each point in the input cloud + * \param[in] True if the mls results should be stored, otherwise false. + */ + inline void + setCacheMLSResults (bool cache_mls_results) { cache_mls_results_ = cache_mls_results; } + + /** \brief Get the cache_mls_results_ value (True if the mls results should be stored, otherwise false). */ + inline bool + getCacheMLSResults () const { return cache_mls_results_; } + + /** \brief Get the MLSResults for input cloud + * \note The results are only stored if setCacheMLSResults(true) was called + * or when using the upsampling method DISTINCT_CLOUD or VOXEL_GRID_DILATION. + */ + inline const std::vector& getMLSResults() const { return mls_results_; } + /** \brief Base method for surface reconstruction for all points given in * \param[out] output the resultant reconstructed surface model */ @@ -341,28 +380,8 @@ namespace pcl */ int desired_num_points_in_radius_; - - /** \brief Data structure used to store the results of the MLS fitting - * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling - */ - struct MLSResult - { - MLSResult () : mean (), plane_normal (), u_axis (), v_axis (), c_vec (), num_neighbors (), curvature (), valid (false) {} - - MLSResult (const Eigen::Vector3d &a_mean, - const Eigen::Vector3d &a_plane_normal, - const Eigen::Vector3d &a_u, - const Eigen::Vector3d &a_v, - const Eigen::VectorXd a_c_vec, - const int a_num_neighbors, - const float &a_curvature); - - Eigen::Vector3d mean, plane_normal, u_axis, v_axis; - Eigen::VectorXd c_vec; - int num_neighbors; - float curvature; - bool valid; - }; + /** \brief True if the mls results for the input cloud should be stored */ + bool cache_mls_results_; /** \brief Stores the MLS result for each point in the input cloud * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling @@ -549,6 +568,7 @@ namespace pcl using MovingLeastSquares::order_; using MovingLeastSquares::compute_normals_; using MovingLeastSquares::upsample_method_; + using MovingLeastSquares::cache_mls_results_; using MovingLeastSquares::VOXEL_GRID_DILATION; using MovingLeastSquares::DISTINCT_CLOUD; From b8e0c564586abc3b299b69ad56ef716f05f68fa0 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 1 Aug 2017 07:21:59 -0500 Subject: [PATCH 2/4] Move struct MLSResult outside the MoveingLeastSquares class. --- surface/include/pcl/surface/impl/mls.hpp | 15 ++++---- surface/include/pcl/surface/mls.h | 48 ++++++++++++------------ 2 files changed, 31 insertions(+), 32 deletions(-) diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index a346befff92..dd24e9a513b 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -704,14 +704,13 @@ pcl::MovingLeastSquares::performUpsampling (PointCloudOut & } ////////////////////////////////////////////////////////////////////////////////////////////// -template -pcl::MovingLeastSquares::MLSResult::MLSResult (const Eigen::Vector3d &a_mean, - const Eigen::Vector3d &a_plane_normal, - const Eigen::Vector3d &a_u, - const Eigen::Vector3d &a_v, - const Eigen::VectorXd a_c_vec, - const int a_num_neighbors, - const float &a_curvature) : +pcl::MLSResult::MLSResult (const Eigen::Vector3d &a_mean, + const Eigen::Vector3d &a_plane_normal, + const Eigen::Vector3d &a_u, + const Eigen::Vector3d &a_v, + const Eigen::VectorXd a_c_vec, + const int a_num_neighbors, + const float &a_curvature) : mean (a_mean), plane_normal (a_plane_normal), u_axis (a_u), v_axis (a_v), c_vec (a_c_vec), num_neighbors (a_num_neighbors), curvature (a_curvature), valid (true) { diff --git a/surface/include/pcl/surface/mls.h b/surface/include/pcl/surface/mls.h index 658d067a119..4aea917eb07 100644 --- a/surface/include/pcl/surface/mls.h +++ b/surface/include/pcl/surface/mls.h @@ -52,6 +52,28 @@ namespace pcl { + /** \brief Data structure used to store the results of the MLS fitting + * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling + */ + struct MLSResult + { + MLSResult () : mean (), plane_normal (), u_axis (), v_axis (), c_vec (), num_neighbors (), curvature (), valid (false) {} + + MLSResult (const Eigen::Vector3d &a_mean, + const Eigen::Vector3d &a_plane_normal, + const Eigen::Vector3d &a_u, + const Eigen::Vector3d &a_v, + const Eigen::VectorXd a_c_vec, + const int a_num_neighbors, + const float &a_curvature); + + Eigen::Vector3d mean, plane_normal, u_axis, v_axis; + Eigen::VectorXd c_vec; + int num_neighbors; + float curvature; + bool valid; + }; + /** \brief MovingLeastSquares represent an implementation of the MLS (Moving Least Squares) algorithm * for data smoothing and improved normal estimation. It also contains methods for upsampling the * resulting cloud based on the parametric fit. @@ -91,28 +113,6 @@ namespace pcl enum UpsamplingMethod {NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION}; - /** \brief Data structure used to store the results of the MLS fitting - * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling - */ - struct MLSResult - { - MLSResult () : mean (), plane_normal (), u_axis (), v_axis (), c_vec (), num_neighbors (), curvature (), valid (false) {} - - MLSResult (const Eigen::Vector3d &a_mean, - const Eigen::Vector3d &a_plane_normal, - const Eigen::Vector3d &a_u, - const Eigen::Vector3d &a_v, - const Eigen::VectorXd a_c_vec, - const int a_num_neighbors, - const float &a_curvature); - - Eigen::Vector3d mean, plane_normal, u_axis, v_axis; - Eigen::VectorXd c_vec; - int num_neighbors; - float curvature; - bool valid; - }; - /** \brief Empty constructor. */ MovingLeastSquares () : CloudSurfaceProcessing (), normals_ (), @@ -317,8 +317,8 @@ namespace pcl getCacheMLSResults () const { return cache_mls_results_; } /** \brief Get the MLSResults for input cloud - * \note The results are only stored if setCacheMLSResults(true) was called - * or when using the upsampling method DISTINCT_CLOUD or VOXEL_GRID_DILATION. + * \note The results are only stored if setCacheMLSResults(true) was called or when using the upsampling method DISTINCT_CLOUD or VOXEL_GRID_DILATION. + * \note This vector is align with the input cloud indicies, so use getCorrespondingIndices to get the correct results when using output cloud indicies. */ inline const std::vector& getMLSResults() const { return mls_results_; } From 21721ccea259aeacc3f30e0d1e1ec9b01f5dd2d2 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 3 Aug 2017 23:03:05 -0500 Subject: [PATCH 3/4] Add documentation for MLSResults and cache MLS results by default --- surface/include/pcl/surface/impl/mls.hpp | 20 +++++------ surface/include/pcl/surface/mls.h | 46 +++++++++++++----------- 2 files changed, 35 insertions(+), 31 deletions(-) diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index dd24e9a513b..efa424765f7 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -121,14 +121,15 @@ pcl::MovingLeastSquares::process (PointCloudOut &output) } case (VOXEL_GRID_DILATION): case (DISTINCT_CLOUD): - { - cache_mls_results_ = true; - break; - } + { + if (!cache_mls_results_) + PCL_WARN("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n"); + + cache_mls_results_ = true; + break; + } default: - { - break; - } + break; } if (cache_mls_results_) @@ -404,11 +405,8 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, break; } - case (VOXEL_GRID_DILATION): - case (DISTINCT_CLOUD): - { + default: break; - } } } diff --git a/surface/include/pcl/surface/mls.h b/surface/include/pcl/surface/mls.h index 4aea917eb07..1edf515f580 100644 --- a/surface/include/pcl/surface/mls.h +++ b/surface/include/pcl/surface/mls.h @@ -52,12 +52,10 @@ namespace pcl { - /** \brief Data structure used to store the results of the MLS fitting - * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling - */ + /** \brief Data structure used to store the results of the MLS fitting */ struct MLSResult { - MLSResult () : mean (), plane_normal (), u_axis (), v_axis (), c_vec (), num_neighbors (), curvature (), valid (false) {} + MLSResult () : num_neighbors (0), curvature (0.0f), valid (false) {} MLSResult (const Eigen::Vector3d &a_mean, const Eigen::Vector3d &a_plane_normal, @@ -67,11 +65,14 @@ namespace pcl const int a_num_neighbors, const float &a_curvature); - Eigen::Vector3d mean, plane_normal, u_axis, v_axis; - Eigen::VectorXd c_vec; - int num_neighbors; - float curvature; - bool valid; + Eigen::Vector3d mean; /**< \brief The mean point of all the neighbors. */ + Eigen::Vector3d plane_normal; /**< \brief The normal of the local plane of the query point. */ + Eigen::Vector3d u_axis; /**< \brief The axis corresponding to the u-coordinates of the local plane of the query point. */ + Eigen::Vector3d v_axis; /**< \brief The axis corresponding to the v-coordinates of the local plane of the query point. */ + 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 */ + int num_neighbors; /**< \brief The number of neighbors used to create the mls surface. */ + float curvature; /**< \brief The curvature at the query point. */ + bool valid; /**< \brief If True, the mls results data is valid, otherwise False. */ }; /** \brief MovingLeastSquares represent an implementation of the MLS (Moving Least Squares) algorithm @@ -128,7 +129,7 @@ namespace pcl upsampling_radius_ (0.0), upsampling_step_ (0.0), desired_num_points_in_radius_ (0), - cache_mls_results_ (false), + cache_mls_results_ (true), mls_results_ (), voxel_size_ (1.0), dilation_iteration_num_ (0), @@ -308,6 +309,8 @@ namespace pcl /** \brief Set wether the mls results should be stored for each point in the input cloud * \param[in] True if the mls results should be stored, otherwise false. + * \note The cache_mls_results_ is forced to true when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD. + * \note If memory consumption is a concern set to false when not using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD. */ inline void setCacheMLSResults (bool cache_mls_results) { cache_mls_results_ = cache_mls_results; } @@ -380,7 +383,9 @@ namespace pcl */ int desired_num_points_in_radius_; - /** \brief True if the mls results for the input cloud should be stored */ + /** \brief True if the mls results for the input cloud should be stored + * \note This is forced to true when using upsampling methods VOXEL_GRID_DILATION or DISTINCT_CLOUD. + */ bool cache_mls_results_; /** \brief Stores the MLS result for each point in the input cloud @@ -560,15 +565,6 @@ namespace pcl typedef boost::shared_ptr > Ptr; typedef boost::shared_ptr > ConstPtr; - using PCLBase::input_; - using PCLBase::indices_; - using MovingLeastSquares::normals_; - using MovingLeastSquares::corresponding_input_indices_; - using MovingLeastSquares::nr_coeff_; - using MovingLeastSquares::order_; - using MovingLeastSquares::compute_normals_; - using MovingLeastSquares::upsample_method_; - using MovingLeastSquares::cache_mls_results_; using MovingLeastSquares::VOXEL_GRID_DILATION; using MovingLeastSquares::DISTINCT_CLOUD; @@ -597,6 +593,16 @@ namespace pcl } protected: + using PCLBase::input_; + using PCLBase::indices_; + using MovingLeastSquares::normals_; + using MovingLeastSquares::corresponding_input_indices_; + using MovingLeastSquares::nr_coeff_; + using MovingLeastSquares::order_; + using MovingLeastSquares::compute_normals_; + using MovingLeastSquares::upsample_method_; + using MovingLeastSquares::cache_mls_results_; + /** \brief Abstract surface reconstruction method. * \param[out] output the result of the reconstruction */ From 90b1b7c868025ad2c4bbc381c24b2f511ce19fa9 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 3 Aug 2017 23:07:38 -0500 Subject: [PATCH 4/4] Make MovingLeastSqures getter methods const --- surface/include/pcl/surface/mls.h | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/surface/include/pcl/surface/mls.h b/surface/include/pcl/surface/mls.h index 1edf515f580..5c8a7e78d60 100644 --- a/surface/include/pcl/surface/mls.h +++ b/surface/include/pcl/surface/mls.h @@ -163,7 +163,7 @@ namespace pcl /** \brief Get a pointer to the search method used. */ inline KdTreePtr - getSearchMethod () { return (tree_); } + getSearchMethod () const { return (tree_); } /** \brief Set the order of the polynomial to be fit. * \param[in] order the order of the polynomial @@ -173,7 +173,7 @@ namespace pcl /** \brief Get the order of the polynomial to be fit. */ inline int - getPolynomialOrder () { return (order_); } + getPolynomialOrder () const { return (order_); } /** \brief Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimation. * \param[in] polynomial_fit set to true for polynomial fit @@ -183,7 +183,7 @@ namespace pcl /** \brief Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial). */ inline bool - getPolynomialFit () { return (polynomial_fit_); } + getPolynomialFit () const { return (polynomial_fit_); } /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting. * \param[in] radius the sphere radius that is to contain all k-nearest neighbors @@ -194,7 +194,7 @@ namespace pcl /** \brief Get the sphere radius used for determining the k-nearest neighbors. */ inline double - getSearchRadius () { return (search_radius_); } + getSearchRadius () const { return (search_radius_); } /** \brief Set the parameter used for distance based weighting of neighbors (the square of the search radius works * best in general). @@ -235,7 +235,7 @@ namespace pcl /** \brief Get the distinct cloud used for the DISTINCT_CLOUD upsampling method. */ inline PointCloudInConstPtr - getDistinctCloud () { return distinct_cloud_; } + getDistinctCloud () const { return distinct_cloud_; } /** \brief Set the radius of the circle in the local point plane that will be sampled @@ -249,7 +249,7 @@ namespace pcl * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling */ inline double - getUpsamplingRadius () { return upsampling_radius_; } + getUpsamplingRadius () const { return upsampling_radius_; } /** \brief Set the step size for the local plane sampling * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling @@ -263,7 +263,7 @@ namespace pcl * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling */ inline double - getUpsamplingStepSize () { return upsampling_step_; } + getUpsamplingStepSize () const { return upsampling_step_; } /** \brief Set the parameter that specifies the desired number of points within the search radius * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling @@ -278,7 +278,7 @@ namespace pcl * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling */ inline int - getPointDensity () { return desired_num_points_in_radius_; } + getPointDensity () const { return desired_num_points_in_radius_; } /** \brief Set the voxel size for the voxel grid * \note Used only in the VOXEL_GRID_DILATION upsampling method @@ -292,7 +292,7 @@ namespace pcl * \note Used only in the VOXEL_GRID_DILATION upsampling method */ inline float - getDilationVoxelSize () { return voxel_size_; } + getDilationVoxelSize () const { return voxel_size_; } /** \brief Set the number of dilation steps of the voxel grid * \note Used only in the VOXEL_GRID_DILATION upsampling method @@ -305,7 +305,7 @@ namespace pcl * \note Used only in the VOXEL_GRID_DILATION upsampling method */ inline int - getDilationIterations () { return dilation_iteration_num_; } + getDilationIterations () const { return dilation_iteration_num_; } /** \brief Set wether the mls results should be stored for each point in the input cloud * \param[in] True if the mls results should be stored, otherwise false. @@ -335,7 +335,7 @@ namespace pcl /** \brief Get the set of indices with each point in output having the * corresponding point in input */ inline PointIndicesPtr - getCorrespondingIndices () { return (corresponding_input_indices_); } + getCorrespondingIndices () const { return (corresponding_input_indices_); } protected: /** \brief The point cloud that will hold the estimated normals, if set. */