diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index f71669a1dfc..be0c5fe81d2 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -186,35 +186,8 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, { case (NONE): { - MLSResult::MLSProjectionResults proj; - proj.normal = mls_result.plane_normal; - proj.point = mls_result.mean; - if (polynomial_fit_) - { - if (projection_method_ == SIMPLE) - { - if (mls_result.num_neighbors >= nr_coeff_ && pcl_isfinite (mls_result.c_vec[0])) - { - proj.point += (mls_result.c_vec[0] * mls_result.plane_normal); - - // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1] - if (compute_normals_) - { - proj.normal = mls_result.plane_normal - mls_result.c_vec[order_ + 1] * mls_result.u_axis - mls_result.c_vec[1] * mls_result.v_axis; - proj.normal.normalize(); - } - } - } - else if (projection_method_ == ORTHOGONAL) - { - double u, v, w; - mls_result.getMLSCoordinates (mls_result.query_point, u, v ,w); - proj = mls_result.projectPointOrthogonalToPolynomialSurface (u, v, w); - } - } - + MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint(projection_method_, compute_normals_, nr_coeff_); addProjectedPointNormal(index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices); - break; } @@ -225,9 +198,7 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, for (float v_disp = -static_cast (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast (upsampling_step_)) if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_) { - MLSResult::MLSProjectionResults proj; - proj = mls_result.projectPointSimpleToPolynomialSurface(u_disp, v_disp); - + MLSResult::MLSProjectionResults proj = mls_result.projectPointSimpleToPolynomialSurface(u_disp, v_disp); addProjectedPointNormal(index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices); } break; @@ -242,35 +213,7 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, if (num_points_to_add <= 0) { // Just add the current point - MLSResult::MLSProjectionResults proj; - proj.normal = mls_result.plane_normal; - proj.point = mls_result.mean; - if (polynomial_fit_) - { - if (projection_method_ == SIMPLE) - { - if (mls_result.num_neighbors >= nr_coeff_ && pcl_isfinite (mls_result.c_vec[0])) - { - // Projection onto MLS surface along Darboux normal to the height at (0,0) - proj.point += (mls_result.c_vec[0] * mls_result.plane_normal); - - // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1] - if (compute_normals_) - { - proj.normal = mls_result.plane_normal - mls_result.c_vec[order_ + 1] * mls_result.u_axis - mls_result.c_vec[1] * mls_result.v_axis; - proj.normal.normalize(); - } - } - } - else if (projection_method_ == ORTHOGONAL) - { - double u, v, w; - mls_result.getMLSCoordinates (mls_result.query_point, u, v, w); - proj = mls_result.projectPointOrthogonalToPolynomialSurface (u, v, w); - break; - } - } - + MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint(projection_method_, compute_normals_, nr_coeff_); addProjectedPointNormal(index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices); } else @@ -480,24 +423,7 @@ pcl::MovingLeastSquares::performUpsampling (PointCloudOut & continue; Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast (); - double u, v, w; - mls_results_[input_index].getMLSCoordinates (add_point, u, v, w); - - MLSResult::MLSProjectionResults proj; - if (polynomial_fit_ && mls_results_[input_index].num_neighbors >= 5 * nr_coeff_) - { - if (projection_method_ == SIMPLE) - proj = mls_results_[input_index].projectPointSimpleToPolynomialSurface (u, v); - else if (projection_method_ == ORTHOGONAL) - proj = mls_results_[input_index].projectPointOrthogonalToPolynomialSurface (u, v, w); - else - proj = mls_results_[input_index].projectPointToMLSPlane (u, v); - } - else - { - proj = mls_results_[input_index].projectPointToMLSPlane (u, v); - } - + MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_); addProjectedPointNormal(input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_); } } @@ -534,24 +460,7 @@ pcl::MovingLeastSquares::performUpsampling (PointCloudOut & continue; Eigen::Vector3d add_point = p.getVector3fMap ().template cast (); - double u, v, w; - mls_results_[input_index].getMLSCoordinates (add_point, u, v, w); - - MLSResult::MLSProjectionResults proj; - if (polynomial_fit_ && mls_results_[input_index].num_neighbors >= 5 * nr_coeff_) - { - if (projection_method_ == SIMPLE) - proj = mls_results_[input_index].projectPointSimpleToPolynomialSurface (u, v); - else if (projection_method_ == ORTHOGONAL) - proj = mls_results_[input_index].projectPointOrthogonalToPolynomialSurface (u, v, w); - else - proj = mls_results_[input_index].projectPointToMLSPlane (u, v); - } - else - { - proj = mls_results_[input_index].projectPointToMLSPlane (u, v); - } - + MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint(add_point, projection_method_, 5 * nr_coeff_); addProjectedPointNormal(input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_); } } @@ -566,10 +475,9 @@ pcl::MLSResult::MLSResult (const Eigen::Vector3d &a_query_point, const Eigen::VectorXd &a_c_vec, const int a_num_neighbors, const float a_curvature, - const int a_order, - const bool a_polynomial_fit): + const int a_order): query_point (a_query_point), 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), order (a_order), polynomial_fit(a_polynomial_fit), valid (true) + curvature (a_curvature), order (a_order), valid (true) { } @@ -662,7 +570,7 @@ Eigen::Vector2f pcl::MLSResult::calculatePrincipleCurvatures (const double u, co // Then: // k1 = H + sqrt(H^2 - K) // k1 = H - sqrt(H^2 - K) - if (polynomial_fit && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0])) + if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0])) { PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v); double Z = 1 + d.z_u * d.z_u + d.z_v * d.z_v; @@ -693,7 +601,7 @@ pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPointOrthogonalToPol MLSProjectionResults result; result.normal = plane_normal; - if (polynomial_fit && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0])) + if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0])) { PolynomialPartialDerivative d = getPolynomialPartialDerivative (gu, gv); gw = d.z; @@ -765,7 +673,7 @@ pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPointSimpleToPolynom result.normal = plane_normal; - if (polynomial_fit && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0])) + if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0])) { PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v); w = d.z; @@ -778,6 +686,60 @@ pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPointSimpleToPolynom return result; } +pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors) const +{ + double u, v, w; + getMLSCoordinates (pt, u, v, w); + + MLSResult::MLSProjectionResults proj; + if (order > 1 && num_neighbors >= required_neighbors && (method == SIMPLE || method == ORTHOGONAL)) + { + if (method == ORTHOGONAL) + proj = projectPointOrthogonalToPolynomialSurface (u, v, w); + else // SIMPLE + proj = projectPointSimpleToPolynomialSurface (u, v); + } + else + { + proj = projectPointToMLSPlane (u, v); + } + + return proj; +} + +pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectQueryPoint(ProjectionMethod method, bool compute_normal, int required_neighbors) const +{ + MLSResult::MLSProjectionResults proj; + if (order > 1 && num_neighbors >= required_neighbors && pcl_isfinite (c_vec[0]) && (method == SIMPLE || method == ORTHOGONAL)) + { + if (method == ORTHOGONAL) + { + double u, v, w; + getMLSCoordinates (query_point, u, v, w); + proj = projectPointOrthogonalToPolynomialSurface (u, v, w); + } + else // SIMPLE + { + // Projection onto MLS surface along Darboux normal to the height at (0,0) + proj.point += (c_vec[0] * plane_normal); + + // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1] + if (compute_normal) + { + proj.normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis; + proj.normal.normalize(); + } + } + } + else + { + proj.normal = plane_normal; + proj.point = mean; + } + + return proj; +} + template void pcl::computeMLSSurface (const pcl::PointCloud &cloud, int index, @@ -826,7 +788,6 @@ void pcl::computeMLSSurface (const pcl::PointCloud &cloud, // Perform polynomial fit to update point and normal //////////////////////////////////////////////////// mls_result.num_neighbors = static_cast (nn_indices.size ()); - mls_result.polynomial_fit = polynomial_fit; if (polynomial_fit) { mls_result.order = polynomial_order; diff --git a/surface/include/pcl/surface/mls.h b/surface/include/pcl/surface/mls.h index da25a95bc8b..5c9d25a777e 100644 --- a/surface/include/pcl/surface/mls.h +++ b/surface/include/pcl/surface/mls.h @@ -60,6 +60,7 @@ namespace pcl /** \brief Data structure used to store the results of the MLS fitting */ struct MLSResult { + enum ProjectionMethod {SIMPLE, ORTHOGONAL}; /** \brief Data structure used to store the MLS polynomial partial derivatives */ struct PolynomialPartialDerivative @@ -91,8 +92,7 @@ namespace pcl const Eigen::VectorXd &a_c_vec, const int a_num_neighbors, const float a_curvature, - const int a_order, - const bool a_polynomial_fit); + const int a_order); /** \brief Given a point calculate it's 3D location in the MLS frame. * \param[in] pt The point @@ -131,31 +131,36 @@ namespace pcl */ Eigen::Vector2f calculatePrincipleCurvatures (const double u, const double v) const; - /** \brief Project a point orthogonal to the polynomial surface. - * \param[in] u The u-coordinate of the point in local MLS frame. - * \param[in] v The v-coordinate of the point in local MLS frame. - * \param[in] w The w-coordinate of the point in local MLS frame. - * \return The MLSProjectionResults for the input data. - * \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane. - * \note If the optimization diverges it performs a simple projection on to the polynomial surface. - * \note This was implemented based on this https://math.stackexchange.com/questions/1497093/shortest-distance-between-point-and-surface - */ - MLSProjectionResults projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const; - - /** \brief Project a point onto the MLS plane. - * \param[in] u The u-coordinate of the point in local MLS frame. - * \param[in] v The v-coordinate of the point in local MLS frame. - * \return The MLSProjectionResults for the input data. - */ - MLSProjectionResults projectPointToMLSPlane (const double u, const double v) const; - - /** \brief Project a point along the MLS plane normal to the polynomial surface. - * \param[in] u The u-coordinate of the point in local MLS frame. - * \param[in] v The v-coordinate of the point in local MLS frame. - * \return The MLSProjectionResults for the input data. - * \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane. - */ - MLSProjectionResults projectPointSimpleToPolynomialSurface (const double u, const double v) const; + /** \brief Project a point orthogonal to the polynomial surface. + * \param[in] u The u-coordinate of the point in local MLS frame. + * \param[in] v The v-coordinate of the point in local MLS frame. + * \param[in] w The w-coordinate of the point in local MLS frame. + * \return The MLSProjectionResults for the input data. + * \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane. + * \note If the optimization diverges it performs a simple projection on to the polynomial surface. + * \note This was implemented based on this https://math.stackexchange.com/questions/1497093/shortest-distance-between-point-and-surface + */ + MLSProjectionResults projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const; + + /** \brief Project a point onto the MLS plane. + * \param[in] u The u-coordinate of the point in local MLS frame. + * \param[in] v The v-coordinate of the point in local MLS frame. + * \return The MLSProjectionResults for the input data. + */ + MLSProjectionResults projectPointToMLSPlane (const double u, const double v) const; + + /** \brief Project a point along the MLS plane normal to the polynomial surface. + * \param[in] u The u-coordinate of the point in local MLS frame. + * \param[in] v The v-coordinate of the point in local MLS frame. + * \return The MLSProjectionResults for the input data. + * \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane. + */ + MLSProjectionResults projectPointSimpleToPolynomialSurface (const double u, const double v) const; + + MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors = 0) const; + + MLSProjectionResults projectQueryPoint(ProjectionMethod method, bool compute_normal, int required_neighbors = 0) const; + Eigen::Vector3d query_point; /**< \brief The query point about which the mls surface was generated */ Eigen::Vector3d mean; /**< \brief The mean point of all the neighbors. */ @@ -165,8 +170,7 @@ namespace pcl 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. */ - int order; /**< \brief The order of the polynomial used if polynomial_fit = true */ - bool polynomial_fit; /**< \brief If True, a polynomial surface was fitted to the neighbors as the mls surface */ + int order; /**< \brief The order of the polynomial. If order > 1 then use polynomial fit */ bool valid; /**< \brief If True, the mls results data is valid, otherwise False. */ }; @@ -237,7 +241,6 @@ namespace pcl typedef boost::function &, std::vector &)> SearchMethod; enum UpsamplingMethod {NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION}; - enum ProjectionMethod {SIMPLE, ORTHOGONAL}; /** \brief Empty constructor. */ MovingLeastSquares () : CloudSurfaceProcessing (), @@ -256,7 +259,7 @@ namespace pcl desired_num_points_in_radius_ (0), cache_mls_results_ (true), mls_results_ (), - projection_method_ (SIMPLE), + projection_method_ (MLSResult::SIMPLE), voxel_size_ (1.0), dilation_iteration_num_ (0), nr_coeff_ (), @@ -452,11 +455,11 @@ namespace pcl * * ORTHONORMAL - Project the point to the closest point on the polynonomial surface. */ inline void - setProjectionMethod (ProjectionMethod method) { projection_method_ = method; } + setProjectionMethod (MLSResult::ProjectionMethod method) { projection_method_ = method; } /** \brief Get the current projection method being used. */ - inline ProjectionMethod + inline MLSResult::ProjectionMethod getProjectionMethod () const { return projection_method_; } /** \brief Get the MLSResults for input cloud @@ -534,7 +537,7 @@ namespace pcl std::vector mls_results_; /** \brief Parameter that specifies the projection method to be used. */ - ProjectionMethod projection_method_; + MLSResult::ProjectionMethod projection_method_; /** \brief A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling