PCL之计算点云质心---pcl::compute3DCentroid()

质心计算公式

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

其中，为各质点的坐标，为质点对应的质量。

PCL函数原理

计算点云质心时，令上述公式中的即可，则点云质心坐标计算公式如下：

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::Ptr cloud(new pcl::PointCloud);
    // 点云文件地址：https://github.com/PointCloudLibrary/data/blob/master/tutorials/table_scene_lms400.pcd
    if (pcl::io::loadPCDFile(" table_scene_lms400.pcd", *cloud) == -1) { // 读取.pcd文件
        cerr << "can't read file table_scene_lms400.pcd" << 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计算点云质心结果：" << centroid << endl;
    cout << "按照公式计算点云质心结果：" << p_c<< endl;
    // 可视化
    pcl::visualization::PCLVisualizer viewer;
    viewer.addPointCloud(cloud);
    viewer.addCoordinateSystem();
    // 质心坐标
    PointT center;
    center.x = centroid(0);
    center.y = centroid(1);
    center.z = centroid(2);

    viewer.addSphere(center, 0.03, 1, 0, 0, "sphere",0);
    while (!viewer.wasStopped())
    {
        viewer.spinOnce(100);
    }
    return 0;
}

• 结果