（四）利用矩阵对点云进行刚体变换

利用矩阵对点云进行刚体变换

在这篇教程中，我们会学习如何用一个4*4的矩阵对点云进行变换。我们会加载一个点云，然后对其进行刚体变换（旋转加平移），最后显示结果。

程序

/* * 功能： 点云刚体变换 * 头文件： #include <pcl/common/transforms.h> * 功能函数： pcl::transformPointCloud(*pPointCloudIn, *pPointCloudOut, transform_1); */#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);    // 2. 可视窗口初始化    pcl::visualization::PCLVisualizer viewer;    viewer.setCameraFieldOfView(0.785398);      // fov 大概45度    viewer.setBackgroundColor(0.5, 0.5, 0.5);   // 背景设为灰色    viewer.setCameraPosition(        0, 0, 5,                                // 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.5m    transform_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.5m    transform_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;}

结果