Skip to content

Commit

Permalink
avoid div by 0 error in compute3DCentroid and follow docstring by not
Browse files Browse the repository at this point in the history 
changing centroid/covariance matrix if valid points is zero
Fixes PointCloudLibrary#2480
  • Loading branch information
@sthoduka
sthoduka committed May 17, 2020
1 parent 14de3be commit abaa3fe
Showing 1 changed file with 75 additions and 61 deletions.
136 changes: 75 additions & 61 deletions common/include/pcl/common/impl/centroid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,27 +56,30 @@ template <typename PointT, typename Scalar> inline unsigned int
compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
Eigen::Matrix<Scalar, 4, 1> &centroid)
{
// Initialize to 0
centroid.setZero ();

unsigned cp = 0;

// Initialize to 0
Eigen::Matrix<Scalar, 3, 1> accu = Eigen::Matrix<Scalar, 3, 1>::Zero ();

// For each point in the cloud
// If the data is dense, we don't need to check for NaN
while (cloud_iterator.isValid ())
{
// 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<Scalar> ();
++cp;
}
++cloud_iterator;
}
centroid /= static_cast<Scalar> (cp);
centroid[3] = 1;
if (cp)
{
accu /= static_cast<Scalar> (cp);
centroid.head (3) = accu;
centroid[3] = 1;
}

return (cp);
}

Expand All @@ -89,18 +92,17 @@ compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
return (0);

// Initialize to 0
centroid.setZero ();
Eigen::Matrix<Scalar, 3, 1> accu = Eigen::Matrix<Scalar, 3, 1>::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<Scalar> ();
}
centroid /= static_cast<Scalar> (cloud.size ());
accu /= static_cast<Scalar> (cloud.size ());
centroid.head (3) = accu;
centroid[3] = 1;

return (static_cast<unsigned int> (cloud.size ()));
Expand All @@ -113,13 +115,15 @@ compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
if (!isFinite (point))
continue;

centroid[0] += point.x;
centroid[1] += point.y;
centroid[2] += point.z;
accu += point.getVector3fMap ().template cast<Scalar> ();
++cp;
}
centroid /= static_cast<Scalar> (cp);
centroid[3] = 1;
if (cp)
{
accu /= static_cast<Scalar> (cp);
centroid.head (3) = accu;
centroid[3] = 1;
}

return (cp);
}
Expand All @@ -134,35 +138,37 @@ compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
return (0);

// Initialize to 0
centroid.setZero ();
Eigen::Matrix<Scalar, 3, 1> accu = Eigen::Matrix<Scalar, 3, 1>::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<Scalar> ();
}
centroid /= static_cast<Scalar> (indices.size ());
accu /= static_cast<Scalar> (indices.size ());
centroid.head (3) = accu;
centroid[3] = 1;
return (static_cast<unsigned int> (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<Scalar> ();
++cp;
}
centroid /= static_cast<Scalar> (cp);
centroid[3] = 1;
if (cp)
{
accu /= static_cast<Scalar> (cp);
centroid.head (3) = accu;
centroid[3] = 1;
}

return (cp);
}

Expand All @@ -185,7 +191,7 @@ computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
return (0);

// Initialize to 0
covariance_matrix.setZero ();
Eigen::Matrix<Scalar, 3, 3> accu = Eigen::Matrix<Scalar, 3, 3>::Zero ();

unsigned point_count;
// If the data is dense, we don't need to check for NaN
Expand All @@ -200,15 +206,15 @@ computeCovarianceMatrix (const pcl::PointCloud<PointT> &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
Expand All @@ -227,21 +233,25 @@ computeCovarianceMatrix (const pcl::PointCloud<PointT> &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);
}
Expand Down Expand Up @@ -269,7 +279,7 @@ computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
return (0);

// Initialize to 0
covariance_matrix.setZero ();
Eigen::Matrix<Scalar, 3, 3> accu = Eigen::Matrix<Scalar, 3, 3>::Zero ();

std::size_t point_count;
// If the data is dense, we don't need to check for NaN
Expand All @@ -284,15 +294,15 @@ computeCovarianceMatrix (const pcl::PointCloud<PointT> &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
Expand All @@ -311,21 +321,25 @@ computeCovarianceMatrix (const pcl::PointCloud<PointT> &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<unsigned int> (point_count));
}

Expand Down

0 comments on commit abaa3fe

Please sign in to comment.