Covariance matrix calculation is wrong, the centroid must be calculat…

…ed first Eigen values can be negative, better to check for 0 first
PointCloudLibrary · Aug 22, 2016 · d5e860b · d5e860b
1 parent c874d28
commit d5e860b
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 7 deletions.
diff --git a/surface/include/pcl/surface/impl/concave_hull.hpp b/surface/include/pcl/surface/impl/concave_hull.hpp
@@ -129,9 +129,10 @@ pcl::ConcaveHull<PointInT>::reconstruct (PointCloud &output, std::vector<pcl::Ve
 template <typename PointInT> void
 pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std::vector<pcl::Vertices> &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<PointInT>::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<double>::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
@@ -57,14 +57,15 @@ template <typename PointInT> void
 pcl::ConvexHull<PointInT>::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<double>::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3)
     dimension_ = 2;
   else
     dimension_ = 3;
@@ -101,7 +102,9 @@ pcl::ConvexHull<PointInT>::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;