pcl中的源码如下:
template <typename PointT, typename Scalar> inline unsigned int
computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
Eigen::Matrix<Scalar, 4, 1> ¢roid)
{
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
std::size_t point_count;
if (cloud.is_dense)
{
point_count = cloud.size ();
// For each point in the cloud
for (const auto& point: cloud)
{
accu [0] += point.x * point.x;
accu [1] += point.x * point.y;
accu [2] += point.x * point.z;
accu [3] += point.y * point.y; // 4
accu [4] += point.y * point.z; // 5
accu [5] += point.z * point.z; // 8
accu [6] += point.x;
accu [7] += point.y;
accu [8] += point.z;
}
}
else
{
point_count = 0;
for (const auto& point: cloud)
{
if (!isFinite (point))
continue;
accu [0] += point.x * point.x;
accu [1] += point.x * point.y;
accu [2] += point.x * point.z;
accu [3] += point.y * point.y;
accu [4] += point.y * point.z;
accu [5] += point.z * point.z;
accu [6] += point.x;
accu [7] += point.y;
accu [8] += point.z;
++point_count;
}
}
accu /= static_cast<Scalar> (point_count);
if (point_count != 0)
{
//centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0
centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
centroid[3] = 1;
covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
}
return (static_cast<unsigned int> (point_count));
}
这是一个计算点云数据的中心和协方差矩阵的函数。该函数采用模板编程技术,可以处理不同类型的点云数据(例如,XYZ、RGB、XYZRGB 等)。
该函数首先创建了一个大小为 9 的 Eigen::Matrix 对象 accu 作为累加器,并将其初始化为零矩阵。然后,对于点云中的每个点,该函数将点的 x、y、z 坐标及其平方项相应地添加到累加器中。
该函数还包括一些特殊处理逻辑,以处理具有无穷值的点或非稠密点云。如果点云是非稠密的,则将计算限制在具有有限值的点上,并更新点的数量。最后,累加器中的值除以点的数量,得到了各个坐标的平均值。最后,使用平均值计算协方差矩阵和中心点。其中,协方差矩阵是描述随机向量之间关系度量的矩阵,而中心点是点云的平均位置。
此函数对于点云分析和计算对象的几何属性非常实用,例如计算点云的主要方向、法线和曲率等信息。
Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
该行代码创建了一个大小为 9 的 Eigen::Matrix 对象 accu 作为累加器,并将其初始化为零矩阵。此处使用 Eigen::RowMajor 存储顺序,以提高性能。