C++【PCL】利用矩阵对点云进行刚体变换

/*
 * 功能： 点云刚体变换
 * 头文件： #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.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;
}