pcl::compute3DCentroid()计算质心算法原理

质心计算公式

质心指的是质量的中心，认为是物体质量集中于此点的假想点。
通常物体质心坐标$P_c$计算公式如下：

$$P_c=\frac{1}{M}\sum _{i=0}^n{m}_i{r}_i$$

其中，$r_i=(x_i,y_i,z_i),i=1,2,...,n$为各质点的坐标，$m_i$为质点对应的质量。

PCL函数原理

计算点云质心时，令上述公式中的 $m_i=1$即可，则点云质心坐标计算公式如下：
$$P_c=\frac{1}{n}(\sum _{i=0}^n{x}_i,\sum _{i=0}^n{y}_i,\sum _{i=0}^n{z}_i)$$

PLC提供了现有的函数可供调用：

Eigen::Vector4f centroid;  //质心 
pcl::compute3DCentroid(*cloud_smoothed,centroid); // 计算质心

该函数的原理即是使用上述公式计算点云质心坐标，接下来通过代码进行验证。

代码

#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
typedef pcl::PointXYZ PointT;

int main(int argc, char **argv)
{
	// 导入点云
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);

	if (pcl::io::loadPLYFile("bunny.ply", *cloud) == -1) { // 读取.ply文件
		cerr << "can't read file bunny.ply" << endl;
		return -1;
	}

	// PCL函数计算质心
	Eigen::Vector4f centroid;					// 质心
	pcl::compute3DCentroid(*cloud, centroid);	// 齐次坐标，（c0,c1,c2,1）
	
	// 按公式计算质心
	PointT p_c;
	p_c.x = 0; p_c.y = 0; p_c.z = 0;
	for (auto p : cloud->points) {
		p_c.x += p.x;
		p_c.y += p.y;
		p_c.z += p.z;
	}

	p_c.x /= cloud->points.size();
	p_c.y /= cloud->points.size();
	p_c.z /= cloud->points.size();

	// 结果对比
	cout << "pcl::compute3DCentroid(*cloud, centroid)计算点云质心结果：" << endl 
		<< "\t" << centroid(0) << "\t" << centroid(1) << "\t" << centroid(2) << "\t" << centroid(3) << endl;

	cout << "按照公式计算点云质心结果：" << endl 
		<< "\t" << p_c.x << "\t" << p_c.y << "\t" << p_c.z << endl;
	
	getchar();
	return 0;
}

结果

备注

点云文件可从博客：PCL+VS2015实现点云可视化中提供的链接下载。