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 16, 2020
1 parent 14de3be commit 3c5591d
Showing 1 changed file with 75 additions and 58 deletions.
133 changes: 75 additions & 58 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,31 @@ 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::Vector4f accu = Eigen::Vector4f::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->getVector4fMap();
++cp;
}
++cloud_iterator;
}
centroid /= static_cast<Scalar> (cp);
if (cp == 0)
{
return (cp);
}
accu /= static_cast<Scalar> (cp);
centroid.col(0) = accu.cast<Scalar>();
centroid[3] = 1;

return (cp);
}

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

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

return (static_cast<unsigned int> (cloud.size ()));
Expand All @@ -113,12 +116,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.getVector4fMap();
++cp;
}
centroid /= static_cast<Scalar> (cp);
if (cp == 0)
{
return (cp);
}
accu /= static_cast<Scalar> (cp);
centroid.col(0) = accu.cast<Scalar>();
centroid[3] = 1;

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

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

return (cp);
}

Expand All @@ -185,7 +194,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 +209,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 +236,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 +282,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 +297,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 +324,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 3c5591d

Please sign in to comment.