【PCL】（六）点云矩阵变换

以下代码实现使用Eigen库定义的4x4矩阵对从PCD或PLY文件加载的点云进行旋转和平移变换，并显示原始和变换后的点云。

matrix_transform.cpp：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>

// 展示帮助信息
void showHelp(char * program_name)
{
  std::cout << std::endl;
  std::cout << "Usage: " << program_name << " cloud_filename.[pcd|ply]" << std::endl;
  std::cout << "-h:  Show this help." << std::endl;
}


int main (int argc, char** argv)
{
  // 命令行上使用-h或-help，将显示帮助信息
  if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) {
    showHelp (argv[0]);
    return 0;
  }

 // 如果在参数中没找到.ply或.pcd文件名，也将显示帮助。
 // file_is_pcd将帮助我们在加载pcd或PLY文件之间进行选择。
  std::vector<int> filenames;
  bool file_is_pcd = false;

  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".ply");
  if (filenames.size () != 1)  {
    filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
    if (filenames.size () != 1) {
      showHelp (argv[0]);
      return -1;
    } else {
      file_is_pcd = true;
    }
  }
  
 // 加载PCD/PLY文件，并检查该文件是否已成功加载
  pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());

  if (file_is_pcd) {
    if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0)  {
      std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
      showHelp (argv[0]);
      return -1;
    }
  } else {
    if (pcl::io::loadPLYFile (argv[filenames[0]], *source_cloud) < 0)  {
      std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
      showHelp (argv[0]);
      return -1;
    }
  }


  /* ========== METHOD #1:使用Matrix4f ==========  */
  // 使用Eigen库将4x4矩阵初始化为单位矩阵；
  Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();

  // 定义其中的旋转矩阵，绕z轴旋转 PI/4
  float theta = M_PI/4; // 旋转的角度
  transform_1 (0,0) = std::cos (theta);
  transform_1 (0,1) = -sin(theta);
  transform_1 (1,0) = sin (theta);
  transform_1 (1,1) = std::cos (theta);
  //    (row, column)

  // 在x轴上定义10米的平移。
  transform_1 (0,3) = 10;

  // 打印变换矩阵
  printf ("Method #1: using a Matrix4f\n");
  std::cout << transform_1 << std::endl;
 /*============================================== */


 /* ========== METHOD #2: 使用Affine3f ==========  */
  Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

  // 在x轴上定义10米的平移。
  transform_2.translation() <<10, 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;
 /*============================================== */

  // 将transform_2(或等价的transform_1)矩阵应用于点云source_cloud，并将结果保存在新创建的transformed_cloud中。
  pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);

  // 使用PCLVisualizer可视化结果。原始点云将显示为白色，变换后的点云显示为红色。
  printf(  "\nPoint cloud colors :  white  = original point cloud\n"
      "                        red  = transformed point cloud\n");
  pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");

   // 定义点云RGB颜色
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
  // 我们将点云添加到查看器中，并传递颜色处理程序color handler 
  viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
  viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");

  viewer.addCoordinateSystem (1.0, "cloud", 0);  //  显示坐标轴。
  viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); //  设置背景颜色
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "original_cloud");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "transformed_cloud");

  while (!viewer.wasStopped ()) { // 显示查看器，直到按下“q”键
    viewer.spinOnce ();
  }

  return 0;
}

编译：CMakeLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
project(pcl-matrix_transform)
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (matrix_transform matrix_transform.cpp)
target_link_libraries (matrix_transform ${PCL_LIBRARIES})

运行

PLY样例数据：cube.ply：

生成可执行文件后，传递PCD或PLY文件的路径并运行：

$ ./matrix_transform cube.ply

Method #1: using a Matrix4f
 0.707107 -0.707107         0       2.5
 0.707107  0.707107         0         0
        0         0         1         0
        0         0         0         1

Method #2: using an Affine3f
 0.707107 -0.707107         0        10
 0.707107  0.707107         0         0
        0         0         1         0
        0         0         0         1

Point cloud colors :  white  = original point cloud
                        red  = transformed point cloud