From abaa3fee86dc3d5e41b5593ef1b5bc6994fbec8b Mon Sep 17 00:00:00 2001 From: Santosh Thoduka Date: Sat, 16 May 2020 17:27:56 +0200 Subject: [PATCH] avoid div by 0 error in compute3DCentroid and follow docstring by not changing centroid/covariance matrix if valid points is zero Fixes #2480 --- common/include/pcl/common/impl/centroid.hpp | 136 +++++++++++--------- 1 file changed, 75 insertions(+), 61 deletions(-) diff --git a/common/include/pcl/common/impl/centroid.hpp b/common/include/pcl/common/impl/centroid.hpp index 598e5232477..6c17550b9d6 100644 --- a/common/include/pcl/common/impl/centroid.hpp +++ b/common/include/pcl/common/impl/centroid.hpp @@ -56,11 +56,11 @@ template inline unsigned int compute3DCentroid (ConstCloudIterator &cloud_iterator, Eigen::Matrix ¢roid) { - // Initialize to 0 - centroid.setZero (); - unsigned cp = 0; + // Initialize to 0 + Eigen::Matrix accu = Eigen::Matrix::Zero (); + // For each point in the cloud // If the data is dense, we don't need to check for NaN while (cloud_iterator.isValid ()) @@ -68,15 +68,18 @@ compute3DCentroid (ConstCloudIterator &cloud_iterator, // Check if the point is invalid if (pcl::isFinite (*cloud_iterator)) { - centroid[0] += cloud_iterator->x; - centroid[1] += cloud_iterator->y; - centroid[2] += cloud_iterator->z; + accu += cloud_iterator->getVector3fMap ().template cast (); ++cp; } ++cloud_iterator; } - centroid /= static_cast (cp); - centroid[3] = 1; + if (cp) + { + accu /= static_cast (cp); + centroid.head (3) = accu; + centroid[3] = 1; + } + return (cp); } @@ -89,18 +92,17 @@ compute3DCentroid (const pcl::PointCloud &cloud, return (0); // Initialize to 0 - centroid.setZero (); + Eigen::Matrix accu = Eigen::Matrix::Zero (); // For each point in the cloud // If the data is dense, we don't need to check for NaN if (cloud.is_dense) { for (const auto& point: cloud) { - centroid[0] += point.x; - centroid[1] += point.y; - centroid[2] += point.z; + accu += point.getVector3fMap ().template cast (); } - centroid /= static_cast (cloud.size ()); + accu /= static_cast (cloud.size ()); + centroid.head (3) = accu; centroid[3] = 1; return (static_cast (cloud.size ())); @@ -113,13 +115,15 @@ compute3DCentroid (const pcl::PointCloud &cloud, if (!isFinite (point)) continue; - centroid[0] += point.x; - centroid[1] += point.y; - centroid[2] += point.z; + accu += point.getVector3fMap ().template cast (); ++cp; } - centroid /= static_cast (cp); - centroid[3] = 1; + if (cp) + { + accu /= static_cast (cp); + centroid.head (3) = accu; + centroid[3] = 1; + } return (cp); } @@ -134,35 +138,37 @@ compute3DCentroid (const pcl::PointCloud &cloud, return (0); // Initialize to 0 - centroid.setZero (); + Eigen::Matrix accu = Eigen::Matrix::Zero (); // If the data is dense, we don't need to check for NaN if (cloud.is_dense) { for (const int& index : indices) { - centroid[0] += cloud[index].x; - centroid[1] += cloud[index].y; - centroid[2] += cloud[index].z; + accu += cloud[index].getVector3fMap ().template cast (); } - centroid /= static_cast (indices.size ()); + accu /= static_cast (indices.size ()); + centroid.head (3) = accu; centroid[3] = 1; return (static_cast (indices.size ())); } // NaN or Inf values could exist => check for them - unsigned cp = 0; + unsigned cp = 0; for (const int& index : indices) { // Check if the point is invalid if (!isFinite (cloud [index])) continue; - centroid[0] += cloud[index].x; - centroid[1] += cloud[index].y; - centroid[2] += cloud[index].z; + accu += cloud[index].getVector3fMap ().template cast (); ++cp; } - centroid /= static_cast (cp); - centroid[3] = 1; + if (cp) + { + accu /= static_cast (cp); + centroid.head (3) = accu; + centroid[3] = 1; + } + return (cp); } @@ -185,7 +191,7 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, return (0); // Initialize to 0 - covariance_matrix.setZero (); + Eigen::Matrix accu = Eigen::Matrix::Zero (); unsigned point_count; // If the data is dense, we don't need to check for NaN @@ -200,15 +206,15 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, pt[1] = point.y - centroid[1]; pt[2] = point.z - centroid[2]; - covariance_matrix (1, 1) += pt.y () * pt.y (); - covariance_matrix (1, 2) += pt.y () * pt.z (); + accu (1, 1) += pt.y () * pt.y (); + accu (1, 2) += pt.y () * pt.z (); - covariance_matrix (2, 2) += pt.z () * pt.z (); + accu (2, 2) += pt.z () * pt.z (); pt *= pt.x (); - covariance_matrix (0, 0) += pt.x (); - covariance_matrix (0, 1) += pt.y (); - covariance_matrix (0, 2) += pt.z (); + accu (0, 0) += pt.x (); + accu (0, 1) += pt.y (); + accu (0, 2) += pt.z (); } } // NaN or Inf values could exist => check for them @@ -227,21 +233,25 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, pt[1] = point.y - centroid[1]; pt[2] = point.z - centroid[2]; - covariance_matrix (1, 1) += pt.y () * pt.y (); - covariance_matrix (1, 2) += pt.y () * pt.z (); + accu (1, 1) += pt.y () * pt.y (); + accu (1, 2) += pt.y () * pt.z (); - covariance_matrix (2, 2) += pt.z () * pt.z (); + accu (2, 2) += pt.z () * pt.z (); pt *= pt.x (); - covariance_matrix (0, 0) += pt.x (); - covariance_matrix (0, 1) += pt.y (); - covariance_matrix (0, 2) += pt.z (); + accu (0, 0) += pt.x (); + accu (0, 1) += pt.y (); + accu (0, 2) += pt.z (); ++point_count; } } - covariance_matrix (1, 0) = covariance_matrix (0, 1); - covariance_matrix (2, 0) = covariance_matrix (0, 2); - covariance_matrix (2, 1) = covariance_matrix (1, 2); + accu (1, 0) = accu (0, 1); + accu (2, 0) = accu (0, 2); + accu (2, 1) = accu (1, 2); + if (point_count != 0) + { + covariance_matrix = accu; + } return (point_count); } @@ -269,7 +279,7 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, return (0); // Initialize to 0 - covariance_matrix.setZero (); + Eigen::Matrix accu = Eigen::Matrix::Zero (); std::size_t point_count; // If the data is dense, we don't need to check for NaN @@ -284,15 +294,15 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, pt[1] = cloud[idx].y - centroid[1]; pt[2] = cloud[idx].z - centroid[2]; - covariance_matrix (1, 1) += pt.y () * pt.y (); - covariance_matrix (1, 2) += pt.y () * pt.z (); + accu (1, 1) += pt.y () * pt.y (); + accu (1, 2) += pt.y () * pt.z (); - covariance_matrix (2, 2) += pt.z () * pt.z (); + accu (2, 2) += pt.z () * pt.z (); pt *= pt.x (); - covariance_matrix (0, 0) += pt.x (); - covariance_matrix (0, 1) += pt.y (); - covariance_matrix (0, 2) += pt.z (); + accu (0, 0) += pt.x (); + accu (0, 1) += pt.y (); + accu (0, 2) += pt.z (); } } // NaN or Inf values could exist => check for them @@ -311,21 +321,25 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, pt[1] = cloud[index].y - centroid[1]; pt[2] = cloud[index].z - centroid[2]; - covariance_matrix (1, 1) += pt.y () * pt.y (); - covariance_matrix (1, 2) += pt.y () * pt.z (); + accu (1, 1) += pt.y () * pt.y (); + accu (1, 2) += pt.y () * pt.z (); - covariance_matrix (2, 2) += pt.z () * pt.z (); + accu (2, 2) += pt.z () * pt.z (); pt *= pt.x (); - covariance_matrix (0, 0) += pt.x (); - covariance_matrix (0, 1) += pt.y (); - covariance_matrix (0, 2) += pt.z (); + accu (0, 0) += pt.x (); + accu (0, 1) += pt.y (); + accu (0, 2) += pt.z (); ++point_count; } } - covariance_matrix (1, 0) = covariance_matrix (0, 1); - covariance_matrix (2, 0) = covariance_matrix (0, 2); - covariance_matrix (2, 1) = covariance_matrix (1, 2); + accu (1, 0) = accu (0, 1); + accu (2, 0) = accu (0, 2); + accu (2, 1) = accu (1, 2); + if (point_count != 0) + { + covariance_matrix = accu; + } return (static_cast (point_count)); }