diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index f71669a1dfc..9984d596bfa 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) { } @@ -656,13 +564,13 @@ pcl::MLSResult::PolynomialPartialDerivative pcl::MLSResult::getPolynomialPartial Eigen::Vector2f pcl::MLSResult::calculatePrincipleCurvatures (const double u, const double v) const { - Eigen::Vector2f k(MLS_MINIMUM_PRINCIPLE_CURVATURE, MLS_MINIMUM_PRINCIPLE_CURVATURE); + Eigen::Vector2f k(1e-5, 1e-5); // Note: this use the Monge Patch to derive the Gaussian curvature and Mean Curvature found here http://mathworld.wolfram.com/MongePatch.html // 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; @@ -718,7 +626,7 @@ pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPointOrthogonalToPol J(1, 1) = F2v; Eigen::Vector2d err (e1, e2); - Eigen::MatrixXd update = J.inverse () * err; + Eigen::Vector2d update = J.inverse () * err; gu -= update(0); gv -= update(1); @@ -728,7 +636,7 @@ pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPointOrthogonalToPol err_total = std::sqrt (e1 * e1 + e2 * e2); - } while (err_total > MLS_PROJECTION_CONVERGENCE_TOLERANCE && dist2 < dist1); + } while (err_total > 1e-8 && dist2 < dist1); if (dist2 > dist1) // the optimization was diverging reset the coordinates for simple projection { @@ -738,6 +646,8 @@ pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPointOrthogonalToPol gw = d.z; } + result.u = gu; + result.v = gv; result.normal -= (d.z_u * u_axis + d.z_v * v_axis); result.normal.normalize(); } @@ -763,9 +673,11 @@ pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPointSimpleToPolynom MLSProjectionResults result; double w = 0; + result.u = u; + result.v = v; 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 +690,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 = mean + (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, @@ -799,10 +765,9 @@ void pcl::computeMLSSurface (const pcl::PointCloud &cloud, pcl::computeCovarianceMatrix (cloud, nn_indices, xyz_centroid, covariance_matrix); EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value; EIGEN_ALIGN16 Eigen::Vector3d eigen_vector; - Eigen::Vector4d model_coefficients; + Eigen::Vector4d model_coefficients (0, 0, 0, 0); pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); model_coefficients.head<3> ().matrix () = eigen_vector; - model_coefficients[3] = 0; model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); // Projected query point @@ -811,10 +776,10 @@ void pcl::computeMLSSurface (const pcl::PointCloud &cloud, double distance = mls_result.query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3]; mls_result.mean = mls_result.query_point - distance * model_coefficients.head<3> (); - mls_result.curvature = static_cast (covariance_matrix.trace ()); + mls_result.curvature = covariance_matrix.trace (); // Compute the curvature surface change if (mls_result.curvature != 0) - mls_result.curvature = fabsf (float (eigen_value / double (mls_result.curvature))); + mls_result.curvature = std::abs (eigen_value / mls_result.curvature); // Get a copy of the plane normal easy access mls_result.plane_normal = model_coefficients.head<3> (); @@ -826,7 +791,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..8d0d4b36135 100644 --- a/surface/include/pcl/surface/mls.h +++ b/surface/include/pcl/surface/mls.h @@ -54,12 +54,10 @@ namespace pcl { - const double MLS_PROJECTION_CONVERGENCE_TOLERANCE = 1e-8; - const double MLS_MINIMUM_PRINCIPLE_CURVATURE = 1e-5; - /** \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 @@ -75,6 +73,8 @@ namespace pcl /** \brief Data structure used to store the MLS projection results */ struct MLSProjectionResults { + MLSProjectionResults () : u (0), v(0) {} + double u; /**< \brief The u-coordinate of the projected point in local MLS frame. */ double v; /**< \brief The u-coordinate of the projected point in local MLS frame. */ Eigen::Vector3d point; /**< \brief The projected point. */ @@ -91,8 +91,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 +130,53 @@ 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 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 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. + /** \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 using the specified method. + * \param[in] pt The point to be project. + * \param[in] method The projection method to be used. + * \param[in] required_neighbors The minimun number of neighbors required. + * \note If required_neighbors then any number of neighbors is allowed. + * \note If required_neighbors is not satisfied it projects to the mls plane. * \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. + MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors = 0) const; + + /** + * \brief Project the query point used to generate the mls surface about using the specified method. + * \param[in] method The projection method to be used. + * \param[in] required_neighbors The minimun number of neighbors required. + * \note If required_neighbors then any number of neighbors is allowed. + * \note If required_neighbors is not satisfied it projects to the mls plane. * \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 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 +186,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 +257,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 +275,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_ (), @@ -295,7 +314,18 @@ namespace pcl * \param[in] order the order of the polynomial */ inline void - setPolynomialOrder (int order) { order_ = order; } + setPolynomialOrder (int order) + { + order_ = order; + if (order_ < 2) + { + polynomial_fit_ = false; + } + else + { + polynomial_fit_ = true; + } + } /** \brief Get the order of the polynomial to be fit. */ inline int @@ -305,7 +335,21 @@ namespace pcl * \param[in] polynomial_fit set to true for polynomial fit */ inline void - setPolynomialFit (bool polynomial_fit) { polynomial_fit_ = polynomial_fit; } + setPolynomialFit (bool polynomial_fit) + { + polynomial_fit_ = polynomial_fit; + if (polynomial_fit_) + { + if (order_ < 2) + { + order_ = 2; + } + } + else + { + order_ = 0; + } + } /** \brief Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial). */ inline bool @@ -452,11 +496,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 +578,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 @@ -619,7 +663,6 @@ namespace pcl /** \brief Smooth a given point and its neighborghood using Moving Least Squares. * \param[in] index the index of the query point in the input cloud * \param[in] nn_indices the set of nearest neighbors indices for pt - * \param[in] nn_sqr_dists the set of nearest neighbors squared distances for pt * \param[out] projected_points the set of points projected points around the query point * (in the case of upsampling method NONE, only the query point projected to its own fitted surface will be returned, * in the case of the other upsampling methods, multiple points will be returned)