代码先锋网 代码片段及技术文章聚合

1 原理

对于点云中的每个点 p i p_i pi​，对应的协方差矩阵 C C C 如下：
C = 1 N ⋅ ∑ i = 1 N ⋅ ( p i − p ‾ ) ⋅ ( p i − p ‾ ) T C=\cfrac {1}{N}·\sum_{i=1}^{N}·\left(p_i-\overline p\right)·\left(p_i-\overline p\right)^T C=N1​⋅i=1∑N​⋅(pi​−p​)⋅(pi​−p​)T
C ⋅ V j ⃗ = λ j ⋅ V j ⃗ ， j ∈ 0 ， 1 ， 2 C·\vec{V_j}=\lambda_j·\vec{V_j}，j∈{0，1，2} C⋅Vj​ ​=λj​⋅Vj​ ​，j∈0，1，2

式中， N N N 为点云中点的个数， p ‾ \overline p p​ 为点云质心， λ j \lambda_j λj​ 为协方差矩阵的第 j j j 个特征值， V j ⃗ \vec{V_j} Vj​ ​ 为第 j j j 个特征向量

点云的标准化协方差矩阵就是指将协方差矩阵的每一个元素除以点的个数

此方法在理论上是准确的。但是，使用浮点进行内部计算会降低精度，但会提高效率。

2 代码实现

关键代码：

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

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

完整代码：

#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::computeMeanAndCovarianceMatrix(*cloud, covariance, centeroid);
	cout << "\n->点云的协方差矩阵为：" << endl;
	cout << covariance << endl;
	cout << "\n->点云质心为：" << endl;
	cout << centeroid << endl;
	//================================================================


	return 0;
}

3 结果展示

->加载了 1348 个数据点

->点云的协方差矩阵为：
   20.117   -3.8919   0.21394
  -3.8919  0.755084 -0.179444
  0.21394 -0.179444   8.88523

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