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

cpp 复制代码
/*
 * 功能: 点云刚体变换
 * 头文件: #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;
}
相关推荐
大翻哥哥7 分钟前
Python 2025:量化金融与智能交易的新纪元
开发语言·python·金融
weixin_4378309443 分钟前
使用冰狐智能辅助实现图形列表自动点击:OCR与HID技术详解
开发语言·javascript·ocr
阿巴Jun1 小时前
【数学】线性代数知识点总结
笔记·线性代数·矩阵
鹿鹿学长1 小时前
2025年全国大学生数学建模竞赛(C题) 建模解析|婴儿染色体数学建模|小鹿学长带队指引全代码文章与思路
c语言·开发语言·数学建模
zhousenshan1 小时前
Python爬虫常用框架
开发语言·爬虫·python
利刃大大2 小时前
【高并发内存池】五、页缓存的设计
c++·缓存·项目·内存池
DKPT2 小时前
Java内存区域与内存溢出
java·开发语言·jvm·笔记·学习
C语言小火车3 小时前
【C++八股文】基础知识篇
c++·tcp/ip·const·智能指针·多线程同步·static关键字·c++内存模型
liulilittle3 小时前
IP校验和算法:从网络协议到SIMD深度优化
网络·c++·网络协议·tcp/ip·算法·ip·通信
眠りたいです3 小时前
基于脚手架微服务的视频点播系统-播放控制部分
c++·qt·ui·微服务·云原生·架构·播放器