PCL：compute3DCentroid ❤️ 计算点云质心

Posted 2021-09-13 没事就要敲代码

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了PCL：compute3DCentroid ❤️ 计算点云质心相关的知识，希望对你有一定的参考价值。

文章目录

1 函数原型

  compute3DCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &centroid)；

2 代码实现

给定点云，返回点云质心，数据类型为 Vector4f

#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>);
	pcl::PCDReader reader;	
	if (reader.read("tree.pcd", *cloud) < 0)
	{
		PCL_ERROR("\\a->点云文件不存在！\\n");
		system("pause");
		return -1;
	}
	cout << "->加载了 " << cloud->points.size() << " 个数据点" << endl;
	//==============================================================================


	//-------------------------------- 获取点云质心 ---------------------------------
	Eigen::Vector4f centroid;					//存放质心坐标
	pcl::compute3DCentroid(*cloud, centroid);	//计算点云质心
	cout << "\\n->点云质心为：" 
		 << "(" << centroid[0] << ", " << centroid[1] << ", " << centroid[2] << ")" << endl;
	//==============================================================================

	return 0;
}

3 输出结果

->加载了 5746 个数据点

->点云质心为：(1.6172, 1.74113, 4.83147)

4 源码

template <typename PointT, typename Scalar> inline unsigned int
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 
                        Eigen::Matrix<Scalar, 4, 1> &centroid)
{
  if (cloud.empty ())
    return (0);

  // Initialize to 0
  centroid.setZero ();
  // For each point in the cloud
  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < cloud.size (); ++i)
    {
      centroid[0] += cloud[i].x;
      centroid[1] += cloud[i].y;
      centroid[2] += cloud[i].z;
    }
    centroid /= static_cast<Scalar> (cloud.size ());
    centroid[3] = 1;

    return (static_cast<unsigned int> (cloud.size ()));
  }
  // NaN or Inf values could exist => check for them
  else
  {
    unsigned cp = 0;
    for (size_t i = 0; i < cloud.size (); ++i)
    {
      // Check if the point is invalid
      if (!isFinite (cloud [i]))
        continue;

      centroid[0] += cloud[i].x;
      centroid[1] += cloud[i].y;
      centroid[2] += cloud[i].z;
      ++cp;
    }
    centroid /= static_cast<Scalar> (cp);
    centroid[3] = 1;

    return (cp);
  }
}