PCL：计算点云协方差矩阵

Posted 2021-09-11 没事就要敲代码

文章目录

• • 1 原理
  • 2 代码实现
  • 3 结果展示

1 原理

对于点云中的每个点 $p_i$，对应的协方差矩阵 $C$ 如下：
$$C=\frac{1}{N}\cdot \sum _{i=1}^N\cdot \left(p_i-\overline{p}\right)\cdot {\left(p_i-\overline{p}\right)}^T$$
$$C\cdot \overrightarrow{V_j}={\lambda }_j\cdot \overrightarrow{V_j}，j\in 0，1，2$$

式中，$N$ 为点云中点的个数，$\overline{p}$ 为点云质心，${\lambda }_j$ 为协方差矩阵的第 $j$ 个特征值，$\overrightarrow{V_j}$ 为第 $j$ 个特征向量

2 代码实现

关键代码：

给定输入点云和点云质心，输出协方差矩阵

pcl::computeCovarianceMatrix(*cloud, centeroid, covariance);

完整代码：

#include <iostream>
#include <pcl\\io\\pcd_io.h>
#include <pcl\\common\\centroid.h>	//定义了中心点的估算以及协方差矩阵的计算

using namespace std;

typedef pcl::PointXYZ PointT;

int main()
{
	//---------------------------- 加载点云 --------------------------
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile("test.pcd", *cloud) < 0)
	{
		PCL_ERROR("->点云文件不存在！\\a\\n");
		system("pause");
		return -1;
	}
	cout << "->加载了 " << cloud->points.size() << " 个数据点" << endl;
	//================================================================


	//------------------------ 计算点云协方差矩阵 ---------------------
	Eigen::Matrix3f covariance;					//创建3×3协方差矩阵存储对象
	Eigen::Vector4f centeroid;					//创建用于计算协方差矩阵的点云质心对象
	pcl::compute3DCentroid(*cloud, centeroid);	//计算点云质心
	pcl::computeCovarianceMatrix(*cloud, centeroid, covariance);	//计算点云协方差矩阵
	cout << "\\n->点云质心为：" << endl;
	cout << centeroid << endl;
	cout << "\\n->点云的协方差矩阵为：" << endl;
	cout << covariance << endl;
	//================================================================


	return 0;
}

3 结果展示

->加载了 1348 个数据点

->点云质心为：
6.86964
1.52943
3.56691
      1

->点云的协方差矩阵为：
 27117.7 -5246.28  288.386
-5246.28  1017.85  -241.89
 288.386  -241.89  11977.3