PCL-点云质心识别

问题描述：

最近又重操旧业开始我的点云任务了，增加点云功能模块

代码：

包含的头文件和函数，函数输入参数是点云

#include <iostream>
#include <Eigen/Core>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <thread>
#include <pcl/common/angles.h> // for pcl::deg2rad
#include <pcl/console/parse.h>
#include <pcl/point_cloud.h>

void center(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){


    // 方式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;
      p_c.x = centroid(0);
      p_c.y = centroid(1);
      p_c.z = centroid(2);

      // 可视化
      pcl::PointCloud<pcl::PointXYZ> ::Ptr center(new pcl::PointCloud<pcl::PointXYZ> );
      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,0,0,255);
      pclViewer.addPointCloud(center,Color_center,"center");
      pclViewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,5,"center");

      //pclViewer.addCoordinateSystem();

      pclViewer.setBackgroundColor(0,0,0);
      pclViewer.initCameraParameters();
      while (!pclViewer.wasStopped())
      {
          pclViewer.spinOnce(100);
          std::this_thread::sleep_for(100ms);
      }
}

效果：