PCL中Matrix4f实现点云坐标变化

一、方法原理

参考：https://blog.csdn.net/youngpan1101/article/details/71086500

二、代码实现

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>  
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using namespace std;
// 主函数
int main(int argc, char** argv) {

	// 加载点云数据文件
	pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::io::loadPCDFile<pcl::PointXYZ>("bunny.pcd", *source);
	Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
	transform_1(0, 0) = 0.995870604090736;
	transform_1(0, 1) = 0.0461999293906251;
	transform_1(0, 2) = -0.0781492574005297;
	transform_1(0, 3) = 0.0;

	transform_1(1, 0) = -0.0170114840727817;
	transform_1(1, 1) = 0.940543863547451;
	transform_1(1, 2) = 0.339246002412522;
	transform_1(1, 3) = 0.0;

	transform_1(2, 0) = 0.0891759458463688;
	transform_1(2, 1) = -0.336515686510356;
	transform_1(2, 2) = 0.937445914927826;
	transform_1(2, 3) = 0.0;

	transform_1(3, 0) = 0.0;
	transform_1(3, 1) = 0.0;
	transform_1(3, 2) = 0.0;
	transform_1(3, 3) = 1.0;

	cout << "变换矩阵\n" << transform_1 << std::endl;

	// 执行变换，并将结果保存在新创建的 transformed_cloud 中
	pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>());
	// 可以使用 transform_1 或 transform_2; t它们是一样的
	pcl::transformPointCloud(*source, *t_cloud, transform_1);
	pcl::io::savePCDFileASCII("bunny.pcd", *t_cloud);
	// 可视化

	pcl::visualization::PCLVisualizer viewer("Matrix transformation example");
	// 为点云定义 R,G,B 颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler(source, 255, 0, 0);
	// 输出点云到查看器，使用颜色管理
	viewer.addPointCloud(source, source_cloud_color_handler, "original_cloud");

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler(t_cloud, 0, 255, 0); // 红
	viewer.addPointCloud(t_cloud, transformed_cloud_color_handler, "transformed_cloud");

	// viewer.addCoordinateSystem(1.0, "cloud", 0);
	viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // 设置背景为深灰
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
	//viewer.setPosition(800, 400); // 设置窗口位置
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(1000));
	}

	return (0);
}

三、结果展示