• pcl 使用矩阵变换（旋转、平移）点云

    目录

    1. 场景

    2. 代码详解

    2.1 头文件

    2.2 参数解析

    2.3 解析参数获取pcd和ply文件索引

    2.4 加载pcd和ply点云数据

    2.5 定义变换矩阵

    2.5.1 预备知识

    2.5.2 实操

     2.6 变换

    2.7 可视化

    3. 整合代码

    1. 场景

    定义一个4x4的转换矩阵，去转换点云数据。里面包含的知识点：

    （1）加载点云数据（pcd or ply）；

    （2）转换点云；

    （3）可视化点云数据（设置点云颜色、背景颜色）。

    2. 代码详解

    2.1 头文件

    1. #include 
    2.  
    3. #include 
    4. #include 
    5. #include 
    6. #include   // pcl::console::find_switch
    7. #include   // pcl::transformPointCloud
    8. #include   // pcl::visualization::PCLVisualizer

    2.2 参数解析

    通过命令行指定点云文件，如果没有设置，则会打印help信息提醒。

    1. // This function displays the help
    2. // 用于提醒输出参数
    3. void
    4. showHelp(char* program_name)
    5. {
    6.     std::cout << std::endl;
    7.     std::cout << "Usage: " << program_name << " cloud_filename.[pcd|ply]" << std::endl;
    8.     std::cout << "-h:  Show this help." << std::endl;
    9. }
    10.  
    11. // This is the main function
    12. int 
    13. main(int argc, char** argv)
    14. {
    15.     // Show help，find_switch：argv命令行中是否有-h，有则运行下面函数
    16.     if (pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help")) {
    17.         showHelp(argv[0]);
    18.         return 0;
    19.     }
    20. }

    2.3 解析参数获取pcd和ply文件索引

    1.     // Fetch point cloud filename in arguments | Works with PCD and PLY files
    2.     std::vector<int> filenames_idx;  // 存储文件名索引的数组
    3.     bool file_is_pcd = false;
    4.  
    5.     // 返回文件名索引的数组， filenames[0]是第0个文件名的索引， 
    6.     // argv[filenames[0]]第0个文件名
    7.     filenames_idx = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
    8.     if (filenames_idx.size() != 1) {  // 不是ply文件
    9.         filenames_idx = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
    10.  
    11.         if (filenames_idx.size() != 1) {  // 也不是pcd文件
    12.             showHelp(argv[0]);
    13.             return -1;
    14.         }
    15.         else {
    16.             file_is_pcd = true;
    17.         }
    18.     }

    2.4 加载pcd和ply点云数据

    1.     // Load file | Works with PCD and PLY files
    2.     pcl::PointCloud::Ptr source_cloud(new pcl::PointCloud());
    3.     if (file_is_pcd) {
    4.         if (pcl::io::loadPCDFile(argv[filenames_idx[0]], *source_cloud) < 0) {  // 默认只有一个文件，该文件名：argv[filenames_idx[0]]
    5.             std::cout << "Error loading point cloud " << argv[filenames_idx[0]] << std::endl << std::endl;
    6.             showHelp(argv[0]);
    7.             return -1;
    8.         }
    9.     }
    10.     else {
    11.         if (pcl::io::loadPLYFile(argv[filenames_idx[0]], *source_cloud) < 0) {
    12.             std::cout << "Error loading point cloud " << argv[filenames_idx[0]] << std::endl << std::endl;
    13.             showHelp(argv[0]);
    14.             return -1;
    15.         }
    16.     }

    2.5 定义变换矩阵

    4x4的变换矩阵，前3x3控制旋转，最右边一列的前3行控制平移。

    刚体变换：物体的位置(平移变换)和朝向(旋转变换)发生改变，而形状不变。

    2.5.1 预备知识

    （1）绕x轴逆时针旋转\theta角度 

    \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & cos\theta & -sin\theta & 0 \\ 0 & sin\theta & cos\theta & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = \begin{bmatrix} x \\ ycos\theta - zsin\theta \\ ysin\theta + zcos\theta \\ 1 \end{}" role="presentation">\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & cos\theta & -sin\theta & 0 \\ 0 & sin\theta & cos\theta & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = \begin{bmatrix} x \\ ycos\theta - zsin\theta \\ ysin\theta + zcos\theta \\ 1 \end{}

    （2）绕y轴逆时针旋转\theta角度

    \begin{bmatrix} cos\theta & 0 & sin\theta & 0 \\ 0 & 1 & 0 & 0 \\ -sin\theta & 0 & cos\theta & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = \begin{bmatrix} xcos\theta + zsin\theta \\ y \\ -xsin\theta + zcos\theta \\ 1 \end{}" role="presentation">\begin{bmatrix} cos\theta & 0 & sin\theta & 0 \\ 0 & 1 & 0 & 0 \\ -sin\theta & 0 & cos\theta & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = \begin{bmatrix} xcos\theta + zsin\theta \\ y \\ -xsin\theta + zcos\theta \\ 1 \end{}

    （3）绕z轴逆时针旋转\theta角度 

    \begin{bmatrix} cos\theta& -sin\theta & 0 & 0 \\ sin\theta & cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = \begin{bmatrix} xcos\theta - ysin\theta \\ xsin\theta + ycos\theta \\ z \\ 1 \end{}" role="presentation">\begin{bmatrix} cos\theta& -sin\theta & 0 & 0 \\ sin\theta & cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = \begin{bmatrix} xcos\theta - ysin\theta \\ xsin\theta + ycos\theta \\ z \\ 1 \end{}

    （4）平移

    点A(a,b,c)和点B(x+a,y+b,z+c)，点A沿着AB平移到B.

    \begin{bmatrix} 1 & 0 & 0 & a \\ 0 & 1 & 0 & b \\ 0 & 0 & 1 & c \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = \begin{bmatrix} x + a \\ y + b \\ z + c \\ 1 \end{}" role="presentation">\begin{bmatrix} 1 & 0 & 0 & a \\ 0 & 1 & 0 & b \\ 0 & 0 & 1 & c \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = \begin{bmatrix} x + a \\ y + b \\ z + c \\ 1 \end{}

    2.5.2 实操

    （1）手动定义4x4变换矩阵

     \begin{bmatrix} cos\theta & -sin\theta & 0 & 2.5 \\ sin\theta & cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{}" role="presentation">\begin{bmatrix} cos\theta & -sin\theta & 0 & 2.5 \\ sin\theta & cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{}

    即绕着z轴旋转\theta角度，再沿着x轴平移2.5。

    1.     // 举个例子，说明下转换矩阵是如何工作的。
    2.     /* Reminder: how transformation matrices work :
    3.     最右边一列的前3行是控制平移，所以x,y,z这三个值会平移数据。
    4.     其中，x值控制沿着x轴上的平移
    5.              |-------> This column is the translation。平移
    6.       | 1 0 0 x |  \
    7.       | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left。旋转。全部是1，则没有变化
    8.       | 0 0 1 z |  /
    9.       | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)。这一行不会改变数据。
    11.       METHOD #1: Using a Matrix4f
    12.       This is the "manual" method, perfect to understand but error prone !
    13.       这个是手动设置的4x4矩阵值，容易理解，但是也容易出错！
    14.     */
    15.  
    16.     // 定义4x4转换矩阵
    17.     // 前面3x3控制旋转，最右列控制平移
    18.     Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();  // 4x4的单位矩阵
    19.     // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
    20.     float theta = M_PI / 4; // The angle of rotation in radians（旋转角度，即弧度）
    21.     transform_1(0, 0) = std::cos(theta);  // 设置这4个值，可以绕z轴旋转theta角度
    22.     transform_1(0, 1) = -sin(theta);
    23.     transform_1(1, 0) = sin(theta);
    24.     transform_1(1, 1) = std::cos(theta);
    25.  
    26.     // Define a translation of 2.5 meters on the x axis.
    27.     transform_1(0, 3) = 2.5;  // 沿着x平移2.5
    28.  
    29.     // Print the transformation
    30.     printf("Method #1: using a Matrix4f\n");
    31.     std::cout << transform_1 << std::endl;

     

     沿着z轴旋转，导致兔子头朝下（右手法则，rgb颜色轴分别是xyz轴）。

    （2）使用Affine3f定义4x4矩阵

    因为是通过函数定义，所以此法更简单，且不容易出错

    1.     Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    2.  
    3.     // Define a translation of 2.5 meters on the x axis.
    4.     // 1，定义平移
    5.     transform_2.translation() << 2.5, 0.0, 0.0;
    6.  
    7.     // The same rotation matrix as before; theta radians around Z axis
    8.     // 2，定义然后Z旋转
    9.     transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));  // UnitZ 即z轴

    两种方法的矩阵值是一样的。 

     2.6 变换

    1.     // Executing the transformation
    2.     // 定义存放变换后的点云对象。
    3.     pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud());
    4.     // You can either apply transform_1 or transform_2; they are the same
    5.     pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);

    2.7 可视化

    1.     // Visualization
    2.     printf("\nPoint cloud colors :  white  = original point cloud\n"
    3.            "                        red  = transformed point cloud\n");
    4.     pcl::visualization::PCLVisualizer viewer("Matrix transformation example");  // 定义可视化对象
    5.  
    6.     // 1，设置原始点云颜色
    7.     // Define R,G,B colors for the point cloud
    8.     pcl::visualization::PointCloudColorHandlerCustom source_cloud_color_handler(source_cloud, 255, 255, 255);
    9.     // We add the point cloud to the viewer and pass the color handler
    10.     viewer.addPointCloud(source_cloud, source_cloud_color_handler, "original_cloud");
    11.     
    12.     // 2，设置新点云颜色
    13.     pcl::visualization::PointCloudColorHandlerCustom transformed_cloud_color_handler(transformed_cloud, 230, 20, 20); // Red
    14.     viewer.addPointCloud(transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
    15.     
    16.     // 3，其他设置：坐标、背景颜色、渲染
    17.     viewer.addCoordinateSystem(1.0, "cloud", 0);
    18.     viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
    19.     viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
    20.     viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
    21.     //viewer.setPosition(800, 400); // Setting visualiser window position
    22.  
    23.     while (!viewer.wasStopped()) { // Display the visualiser until 'q' key is pressed
    24.         viewer.spinOnce();
    25.     }

    3. 整合代码

    1. #include 
    2.  
    3. #include 
    4. #include 
    5. #include 
    6. #include   // pcl::console::find_switch
    7. #include   // pcl::transformPointCloud
    8. #include   // pcl::visualization::PCLVisualizer
    9.  
    10. // This function displays the help
    11. // 用于提醒输出参数
    12. void
    13. showHelp(char* program_name)
    14. {
    15.     std::cout << std::endl;
    16.     std::cout << "Usage: " << program_name << " cloud_filename.[pcd|ply]" << std::endl;
    17.     std::cout << "-h:  Show this help." << std::endl;
    18. }
    19.  
    20. // This is the main function
    21. int 
    22. main(int argc, char** argv)
    23. {
    24.     // Show help，find_switch：argv命令行中是否有-h，有则运行下面函数
    25.     if (pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help")) {
    26.         showHelp(argv[0]);
    27.         return 0;
    28.     }
    29.  
    30.     // Fetch point cloud filename in arguments | Works with PCD and PLY files
    31.     std::vector<int> filenames_idx;  // 存储文件名索引的数组
    32.     bool file_is_pcd = false;
    33.  
    34.     // 返回文件名索引的数组， filenames[0]是第0个文件名的索引， 
    35.     // argv[filenames[0]]第0个文件名
    36.     filenames_idx = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
    37.     if (filenames_idx.size() != 1) {  // 不是ply文件
    38.         filenames_idx = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
    39.  
    40.         if (filenames_idx.size() != 1) {  // 也不是pcd文件
    41.             showHelp(argv[0]);
    42.             return -1;
    43.         }
    44.         else {
    45.             file_is_pcd = true;
    46.         }
    47.     }
    48.  
    49.     // Load file | Works with PCD and PLY files
    50.     pcl::PointCloud::Ptr source_cloud(new pcl::PointCloud());
    51.     if (file_is_pcd) {
    52.         if (pcl::io::loadPCDFile(argv[filenames_idx[0]], *source_cloud) < 0) {  // 默认只有一个文件，该文件名：argv[filenames_idx[0]]
    53.             std::cout << "Error loading point cloud " << argv[filenames_idx[0]] << std::endl << std::endl;
    54.             showHelp(argv[0]);
    55.             return -1;
    56.         }
    57.     }
    58.     else {
    59.         if (pcl::io::loadPLYFile(argv[filenames_idx[0]], *source_cloud) < 0) {
    60.             std::cout << "Error loading point cloud " << argv[filenames_idx[0]] << std::endl << std::endl;
    61.             showHelp(argv[0]);
    62.             return -1;
    63.         }
    64.     }
    65.  
    66.     // 举个例子，说明下转换矩阵是如何工作的。
    67.     /* Reminder: how transformation matrices work :
    68.     最右边一列的前3行是控制平移，所以x,y,z这三个值会平移数据。
    69.     其中，x值控制沿着x轴上的平移
    70.              |-------> This column is the translation。平移
    71.       | 1 0 0 x |  \
    72.       | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left。旋转。全部是1，则没有变化
    73.       | 0 0 1 z |  /
    74.       | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)。这一行不会改变数据。
    76.       METHOD #1: Using a Matrix4f
    77.       This is the "manual" method, perfect to understand but error prone !
    78.       这个是手动设置的4x4矩阵值，容易理解，但是也容易出错！
    79.     */
    80.  
    81.     // 定义4x4转换矩阵
    82.     // 前面3x3控制旋转，最右列控制平移
    83.     Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();  // 4x4的单位矩阵
    84.     // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
    85.     float theta = M_PI / 4; // The angle of rotation in radians（旋转角度，即弧度）
    86.     transform_1(0, 0) = std::cos(theta);  // 设置这4个值，可以绕z轴旋转theta角度
    87.     transform_1(0, 1) = -sin(theta);
    88.     transform_1(1, 0) = sin(theta);
    89.     transform_1(1, 1) = std::cos(theta);
    90.  
    91.     // Define a translation of 2.5 meters on the x axis.
    92.     transform_1(0, 3) = 2.5;  // 沿着x平移2.5
    93.  
    94.     // Print the transformation
    95.     printf("Method #1: using a Matrix4f\n");
    96.     std::cout << transform_1 << std::endl;
    97.  
    98.     /*  METHOD #2: Using a Affine3f
    99.       This method is easier and less error prone
    100.     */
    101.     Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    102.  
    103.     // Define a translation of 2.5 meters on the x axis.
    104.     // 1，定义平移
    105.     transform_2.translation() << 2.5, 0.0, 0.0;
    106.  
    107.     // The same rotation matrix as before; theta radians around Z axis
    108.     // 2，定义然后Z旋转
    109.     transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));  // UnitZ 即z轴
    110.  
    111.     // Print the transformation
    112.     printf("\nMethod #2: using an Affine3f\n");
    113.     std::cout << transform_2.matrix() << std::endl;
    114.  
    115.     // Executing the transformation
    116.     // 定义存放变换后的点云对象。
    117.     pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud());
    118.     // You can either apply transform_1 or transform_2; they are the same
    119.     pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);
    120.  
    121.     // Visualization
    122.     printf("\nPoint cloud colors :  white  = original point cloud\n"
    123.            "                        red  = transformed point cloud\n");
    124.     pcl::visualization::PCLVisualizer viewer("Matrix transformation example");  // 定义可视化对象
    125.  
    126.     // 1，设置原始点云颜色
    127.     // Define R,G,B colors for the point cloud
    128.     pcl::visualization::PointCloudColorHandlerCustom source_cloud_color_handler(source_cloud, 255, 255, 255);
    129.     // We add the point cloud to the viewer and pass the color handler
    130.     viewer.addPointCloud(source_cloud, source_cloud_color_handler, "original_cloud");
    131.     
    132.     // 2，设置新点云颜色
    133.     pcl::visualization::PointCloudColorHandlerCustom transformed_cloud_color_handler(transformed_cloud, 230, 20, 20); // Red
    134.     viewer.addPointCloud(transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
    135.     
    136.     // 3，其他设置：坐标、背景颜色、渲染
    137.     viewer.addCoordinateSystem(1.0, "cloud", 0);
    138.     viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
    139.     viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
    140.     viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
    141.     //viewer.setPosition(800, 400); // Setting visualiser window position
    142.  
    143.     while (!viewer.wasStopped()) { // Display the visualiser until 'q' key is pressed
    144.         viewer.spinOnce();
    145.     }
    146.  
    147.     return 0;
    148. }

    参考：Using a matrix to transform a point cloud — Point Cloud Library 0.0 documentation

  • 相关阅读:
    Web安全—Web漏扫工具Nikto安装与使用
    《MySQL实战45讲》——学习笔记12 “InnoDB刷脏页的控制策略“
    Ubuntu下qt编译问题
    【WSN通信】基于最佳簇半径的无线传感器网络分簇路由算法附matlab代码
    SpringBoot定时任务实现方式--超好用
    数据结构 基数排序的优化
    Zoho Mail荣登福布斯小型企业企业邮箱排行榜
    Lyapunov稳定性分析3（离散时间系统）
    [LeetCode]-138. 随机链表的复制
    net转java学习笔记-sqlserver的问题记录
  • 原文地址：https://blog.csdn.net/jizhidexiaoming/article/details/127653543