PCL去点云质心

日常实验中，从源点云中截取部分点云，其质心往往会不同程度的偏离原点（0，0，0）。为便于实验研究，可将其执行去质心处理。主要分为两个步骤：①求质心；②去质心

本文通过 pcl::compute3DCentroid() 函数求点云质心，然后借助 pcl::demeanPointCloud 实现去质心操作，也可以自己写循环实现，两种方式都可以实现去点云质心。

实现代码：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>	
#include <pcl/common/centroid.h>

using namespace std;

int main()
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZRGB>);

	//加载点云
	if (pcl::io::loadPCDFile("tree.pcd", *cloud_in) < 0)
	{
		PCL_ERROR("该点云文件不存在！\n");
		return -1;
	}

	//计算输入点云质心
	Eigen::Vector4f centroid_in;
	pcl::compute3DCentroid(*cloud_in, centroid_in);
	cout << "zhi心坐标为：\n"
		<< "core_x:" << centroid_in[0] << endl
		<< "core_y:" << centroid_in[1] << endl
		<< "core_z:" << centroid_in[2] << endl;
	

	/*-----执行去质心-----*/
	//方式1：调用demeanPointCloud()函数
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out1(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::demeanPointCloud(*cloud_in, centroid_in, *cloud_out1);
	//计算去质心后点云质心
	Eigen::Vector4f centroid_out1;
	pcl::compute3DCentroid(*cloud_out1, centroid_out1);
	cout << "方式1去质心后质心坐标为：\n"
		<< "core_x:" << centroid_out1[0] << endl
		<< "core_y:" << centroid_out1[1] << endl
		<< "core_z:" << centroid_out1[2] << endl;

	//方式2：循环实现
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out2(new pcl::PointCloud<pcl::PointXYZRGB>);
	*cloud_out2 = *cloud_in;
	for (size_t i = 0; i < cloud_in->size(); ++i)
	{
		cloud_out2->points[i].x = cloud_in->points[i].x - centroid_in[0];
		cloud_out2->points[i].y = cloud_in->points[i].y - centroid_in[1];
		cloud_out2->points[i].z = cloud_in->points[i].z - centroid_in[2];
	}
	//计算去质心后点云质心
	Eigen::Vector4f centroid_out2;
	pcl::compute3DCentroid(*cloud_out2, centroid_out2);
	cout << "方式2去质心后质心坐标为：\n"
		<< "core_x:" << centroid_out2[0] << endl
		<< "core_y:" << centroid_out2[1] << endl
		<< "core_z:" << centroid_out2[2] << endl;

	//两种去重心方式的比较
	cout << "两种去质心方式的差值：" << endl;
	cout << "delt_x=" << centroid_out1[0] - centroid_out2[0] << endl
		<< "delt_y=" << centroid_out1[1] - centroid_out2[1] << endl
		<< "delt_z=" << centroid_out1[2] - centroid_out2[2] << endl;

	//保存去重心后点云
	if (!cloud_out1->empty())
	{
		pcl::io::savePCDFileBinary("cloud0.pcd", *cloud_out1);
	}

	return 0;
}

输出结果：

其实，demeanPointCloud()函数的内部实现就是for循环

template <typename PointT, typename Scalar> void
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                       const Eigen::Matrix<Scalar, 4, 1> &centroid,
                       pcl::PointCloud<PointT> &cloud_out)
{
  cloud_out = cloud_in;

  // Subtract the centroid from cloud_in
  for (size_t i = 0; i < cloud_in.points.size (); ++i)
  {
    cloud_out[i].x -= static_cast<float> (centroid[0]);
    cloud_out[i].y -= static_cast<float> (centroid[1]);
    cloud_out[i].z -= static_cast<float> (centroid[2]);
  }
}