diff --git a/surface/include/pcl/surface/impl/concave_hull.hpp b/surface/include/pcl/surface/impl/concave_hull.hpp index 378b8d9384d..dae1be74703 100644 --- a/surface/include/pcl/surface/impl/concave_hull.hpp +++ b/surface/include/pcl/surface/impl/concave_hull.hpp @@ -129,9 +129,10 @@ pcl::ConcaveHull::reconstruct (PointCloud &output, std::vector void pcl::ConcaveHull::performReconstruction (PointCloud &alpha_shape, std::vector &polygons) { - EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix; Eigen::Vector4d xyz_centroid; - computeMeanAndCovarianceMatrix (*input_, *indices_, covariance_matrix, xyz_centroid); + compute3DCentroid (*input_, *indices_, xyz_centroid); + EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix; + computeCovarianceMatrixNormalized (*input_, *indices_, xyz_centroid, covariance_matrix); // Check if the covariance matrix is finite or not. for (int i = 0; i < 3; ++i) @@ -150,7 +151,7 @@ pcl::ConcaveHull::performReconstruction (PointCloud &alpha_shape, std: if (dim_ == 0) { PCL_DEBUG ("[pcl::%s] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ()); - if (eigen_values[0] / eigen_values[2] < 1.0e-3) + if (std::abs (eigen_values[0]) < std::numeric_limits::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3) dim_ = 2; else dim_ = 3; diff --git a/surface/include/pcl/surface/impl/convex_hull.hpp b/surface/include/pcl/surface/impl/convex_hull.hpp index b6f8db58e05..0af1edca135 100644 --- a/surface/include/pcl/surface/impl/convex_hull.hpp +++ b/surface/include/pcl/surface/impl/convex_hull.hpp @@ -57,14 +57,15 @@ template void pcl::ConvexHull::calculateInputDimension () { PCL_DEBUG ("[pcl::%s::calculateInputDimension] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ()); - EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix; Eigen::Vector4d xyz_centroid; - computeMeanAndCovarianceMatrix (*input_, *indices_, covariance_matrix, xyz_centroid); + compute3DCentroid (*input_, *indices_, xyz_centroid); + EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix; + computeCovarianceMatrixNormalized (*input_, *indices_, xyz_centroid, covariance_matrix); EIGEN_ALIGN16 Eigen::Vector3d eigen_values; pcl::eigen33 (covariance_matrix, eigen_values); - if (eigen_values[0] / eigen_values[2] < 1.0e-3) + if (std::abs (eigen_values[0]) < std::numeric_limits::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3) dimension_ = 2; else dimension_ = 3; @@ -101,7 +102,9 @@ pcl::ConvexHull::performReconstruction2D (PointCloud &hull, std::vecto Eigen::Vector4d normal_calc_centroid; Eigen::Matrix3d normal_calc_covariance; - pcl::computeMeanAndCovarianceMatrix (normal_calc_cloud, normal_calc_covariance, normal_calc_centroid); + pcl::compute3DCentroid (normal_calc_cloud, normal_calc_centroid); + pcl::computeCovarianceMatrixNormalized (normal_calc_cloud, normal_calc_centroid, normal_calc_covariance); + // Need to set -1 here. See eigen33 for explanations. Eigen::Vector3d::Scalar eigen_value; Eigen::Vector3d plane_params;