一、方法原理
参考: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);
}