diff --git a/common/include/pcl/common/impl/centroid.hpp b/common/include/pcl/common/impl/centroid.hpp
index 598e5232477..582ba1bcaaa 100644
--- a/common/include/pcl/common/impl/centroid.hpp
+++ b/common/include/pcl/common/impl/centroid.hpp
@@ -56,11 +56,11 @@ 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 ())
@@ -68,15 +68,19 @@ compute3DCentroid (ConstCloudIterator<PointT> &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->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);
 }
 
@@ -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 ()));
@@ -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);
@@ -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);
 }
 
@@ -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
@@ -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
@@ -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);
 }
@@ -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
@@ -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
@@ -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));
 }