PCL计算均值和协方差矩阵computeMeanAndCovarianceMatrix

最新推荐文章于 2024-10-28 20:23:47 发布

原创

最新推荐文章于 2024-10-28 20:23:47 发布 · 988 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#均值算法 #矩阵 #算法

这是一个用于计算点云数据的中心和协方差矩阵的函数，采用模板编程，适用于不同类型的点云。函数首先初始化累加器accu，然后遍历点云，处理有限值的点，计算平均值和协方差，最后得出中心点和协方差矩阵，用于点云分析和几何属性计算。

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> &centroid)
{
  // 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