/** 功能： 点云刚体变换* 头文件： #include <pcl/common/transforms.h>* 功能函数： pcl::transformPointCloud(*pPointCloudIn, *pPointCloudOut, transform_1);*/#include <pcl/io/ply_io.h>#include <iostream>// pcl
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>int main()
{// 1. 读入点云pcl::PointCloud<pcl::PointXYZ>::Ptr pPointCloudIn(new pcl::PointCloud<pcl::PointXYZ>());//pcl::io::loadPCDFile("Box.pcd", *pPointCloudIn);pcl::io::loadPLYFile<pcl::PointXYZ>("bunny.ply", *pPointCloudIn);// 2. 可视窗口初始化pcl::visualization::PCLVisualizer viewer;viewer.setCameraFieldOfView(0.785398);      // fov 大概45度viewer.setBackgroundColor(0.5, 0.5, 0.5);   // 背景设为灰色viewer.setCameraPosition(0, 0, 3,                                // camera位置0, 0, -1,                               // view向量--相机朝向0, 1, 0                                 // up向量);// 3. 点云旋转/* 提示: 变换矩阵工作原理 :|-------> 这一列表示平移向量| 1 0 0 x |  \| 0 1 0 y |   }-> 左侧的 3x3 matrix表示旋转，这里用无旋转举例| 0 0 1 z |  /| 0 0 0 1 |    -> 这行没有实际意义只是为了计算补充的 (始终为 0,0,0,1)方法 #1: 使用一个Matrix4f类型矩阵（f代表float）这是手工方式，易于理解但是容易出错！*/Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();      // 单位阵// 定义一个旋转矩阵表示绕z轴转PI/4 (从这里可以查看旋转矩阵 https://en.wikipedia.org/wiki/Rotation_matrix)float theta = M_PI / 4; // 旋转的角度，以弧度为单位transform_1(0, 0) = cos(theta);transform_1(0, 1) = -sin(theta);transform_1(1, 0) = sin(theta);transform_1(1, 1) = cos(theta);// 圆括号内的意义(元素在矩阵中的行, 元素在矩阵中的列)// 定义沿着x轴平移2.5mtransform_1(0, 3) = 2.5;// 打印变换矩阵printf("Method #1: using a Matrix4f\n");std::cout << transform_1 << std::endl;/*  方法 #2: 使用一个Affine3f类型这个方法更简单也不容易出错*/Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();// 定义沿着x轴平移2.5mtransform_2.translation() << 2.5, 0.0, 0.0;// 与之前的旋转定义一样，绕z轴旋转一个theta角transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));// 打印变换矩阵printf("\nMethod #2: using an Affine3f\n");std::cout << transform_2.matrix() << std::endl;// 执行变换pcl::PointCloud<pcl::PointXYZ>::Ptr pPointCloudOut(new pcl::PointCloud<pcl::PointXYZ>());// 你可以使用 transform_1 或者 transform_2; 因为他们都一样pcl::transformPointCloud(*pPointCloudIn, *pPointCloudOut, transform_1);// 4. 点云可视化pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> inColorHandler(pPointCloudIn, 255, 255, 255);// 白色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> outColorHandler(pPointCloudOut, 230, 20, 20); // 红色viewer.addPointCloud(pPointCloudIn, inColorHandler, "In");viewer.addPointCloud(pPointCloudOut, outColorHandler, "Out");viewer.addCoordinateSystem(1.0, "cloud", 0);while (!viewer.wasStopped()) { // 显示，直到‘q’键被按下viewer.spinOnce();}std::system("pause");return 0;
}