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]);
  }
}

轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/273348.html

標籤：其他

上一篇：zabbix監控的報警機制

下一篇：Docker快速入門之Docker概念，執行流程以及一些常用指令