简介

本篇用一个4X4的矩阵,对点云数据进行平移和旋转变换,看到自动驾驶中对点云进行变换的时候用到的比较多

思路

1. 引入库函数

#include  
1

该头文件包含变换矩阵的API.
2. 程序的帮助文档,-h,–help都可以激活,写这个函数是一个优秀的习惯:

// This function displays the help
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;
}
...此处省略了很多代码...
// Show help
  if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) {
    showHelp (argv[0]);
    return 0;
  }
1
2
3
4
5
6
7
8
9
10
11
12
13

3. 从命令行接收参数(点云文件格式的)

// Fetch point cloud filename in arguments | Works with PCD and PLY files
  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;
    }
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

4. 初始化一个4X4的矩阵

/* Reminder: how transformation matrices work :

           |-------> This column is the translation
    | 1 0 0 x |  \
    | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
    | 0 0 1 z |  /
    | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)

    METHOD #1: Using a Matrix4f
    This is the "manual" method, perfect to understand but error prone !
  */
  Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
  /*
    |  1  0  0  0  |
i = |  0  1  0  0  |
    |  0  0  1  0  |
    |  0  0  0  1  |
*/
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

5. 为旋转矩阵赋值,它的3阶是旋转矩阵需要的,值得一提的是,这里对旋转矩阵的构造不止一种,选取一种作为示例,其余请读者据官网自行探索.

// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
  float theta = M_PI/4; // The angle of rotation in radians
  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)

  // Define a translation of 2.5 meters on the x axis.
  transform_1 (0,3) = 2.5;

  // Print the transformation
  printf ("Method #1: using a Matrix4f\n");
  std::cout << transform_1 << std::endl;
/*
    |  cos(θ) -sin(θ)  0.0 |
R = |  sin(θ)  cos(θ)  0.0 |
    |  0.0     0.0     1.0 |

t = < 2.5, 0.0, 0.0 >
*/
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

6. 开始变换

// Executing the transformation
  pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  // You can either apply transform_1 or transform_2; they are the same
  pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_1);

1
2
3
4
5

7. 然后就可以通过可视化模块PCLVisualizer,可视出来
8. 最后编译,查看,就可以了,下边是执行效果(记得要带参数即需要变换的点云格式图片执行哦),这里我的点云文件是cube.ply:

./matrix_transform cube.ply 
1

命令行输出

效果pcl输出

源码

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})
1
2
3
4
5
6
7
8
9
10
11
12

matrix_transform.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include 

// This function displays the help
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;
}
// This is the main function
int main (int argc, char** argv)
{

  // Show help
  if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) 
  {
   showHelp (argv[0]);
    return 0;
  }

  // Fetch point cloud filename in arguments | Works with PCD and PLY files
   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;
     }
   }
 
   // Load file | Works with PCD and PLY files
   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;
     }
   }
 
   /* Reminder: how transformation matrices work :
 
            |-------> This column is the translation
     | 1 0 0 x |  \
     | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
     | 0 0 1 z |  /
     | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)
 
     METHOD #1: Using a Matrix4f
     This is the "manual" method, perfect to understand but error prone !
   */
   Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
 
   // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
   float theta = M_PI/4; // The angle of rotation in radians
   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)
 
   // Define a translation of 2.5 meters on the x axis.
   transform_1 (0,3) = 2.5;
 
   // Print the transformation
   printf ("Method #1: using a Matrix4f\n");
   std::cout << transform_1 << std::endl;
 
   /*  METHOD #2: Using a Affine3f
     This method is easier and less error prone
   */
   Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
 
   // Define a translation of 2.5 meters on the x axis.
   transform_2.translation() << 2.5, 0.0, 0.0;
 
  // The same rotation matrix as before; theta radians around Z axis
  transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));

  // Print the transformation
  printf ("\nMethod #2: using an Affine3f\n");
  std::cout << transform_2.matrix() << std::endl;

 // Executing the transformation
 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  // You can either apply transform_1 or transform_2; they are the same
  pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);

  // Visualization
  printf(  "\nPoint cloud colors :  white  = original point cloud\n"
      "                        red  = transformed point cloud\n");
  pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");

   // Define R,G,B colors for the point cloud
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
  // We add the point cloud to the viewer and pass the 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); // Setting background to a dark grey
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
  //viewer.setPosition(800, 400); // Setting visualiser window position

  while (!viewer.wasStopped ()) 
  { // Display the visualiser until 'q' key is pressed
    viewer.spinOnce ();
  }

  return 0;
}


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145