PCL: 获取点云的质心（几何中心点）

  获取点云的质心有两种方式，一种是class="superseo">pcl自带的API：pcl::compute3DCentroid(*cloud, centroid);，另一种是用数学的方式求所有点坐标的平均值。下面的代码实现了这两种方式。

一、C++代码

#include 
#include 
#include 
#include 
#include 
#include 
 
using namespace std;


typedef pcl::PointCloud<pcl::PointXYZ> Cloud;

int main(int argc, char **argv)
{
    // 生成一个正方形点云
    Cloud::Ptr cloud(new Cloud);


	cloud->width = 10000;
	cloud->height = 1;
	cloud->is_dense = false;
	cloud->points.resize(cloud->width * cloud->height);  

    for (int i = 0; i < cloud->points.size(); i++)
    {
        cloud->points[i].x=1000* (rand() / (RAND_MAX + 1.0f));
        cloud->points[i].y=1000* (rand() / (RAND_MAX + 1.0f));
        cloud->points[i].z=0;
    }
    
 
    // 方式1：利用PCL函数计算质心
    Eigen::Vector4f centroid;					// 质心
    pcl::compute3DCentroid(*cloud, centroid);	// 齐次坐标，（c0,c1,c2,1）
 
    // 方式2：利用公式计算质心
    pcl::PointXYZ 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(0)<<","<<centroid(1)<<","<<centroid(2)<<")" << endl;
    cout << "按照公式计算点云质心结果：" << p_c<< endl;


    // 可视化
    Cloud::Ptr center(new Cloud);
    center->points.push_back(p_c);


    pcl::visualization::PCLVisualizer pclViewer("center point");
    
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>Color(cloud,255,255,255);
    pclViewer.addPointCloud(cloud,Color,"cloud");    
    pclViewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"cloud");
    
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>Color_center(center,255,0,0);
    pclViewer.addPointCloud(center,Color_center,"center");    
    pclViewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,10,"center");
    
    //pclViewer.addCoordinateSystem();

    pclViewer.setBackgroundColor(0,0,0);
    pclViewer.initCameraParameters();
    while (!pclViewer.wasStopped())
    {
        pclViewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(1000));
    }

    return 0;
}

二、效果展示

效果如下：