1 #include <iostream>
2 #include <pcl/io/pcd_io.h>
3 #include <pcl/io/ply_io.h>
4 #include <pcl/point_cloud.h>
5 #include <pcl/visualization/pcl_visualizer.h>
6 #include <pcl/common/transforms.h>
7
8 int main()
9 {
10 pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());
11 std::string name = "D:\\pcd\\rabbit.pcd";
12 int ret =pcl::io::loadPCDFile(name, *source_cloud);
13 if (ret < 0)
14 {
15 std::cout << "load file error!!!" << std::endl;
16 return -1;
17 }
18 cout << *source_cloud << std::endl;
19 /* Reminder: how transformation matrices work :
20
21 |-------> This column is the translation
22 | 1 0 0 x | \
23 | 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left
24 | 0 0 1 z | /
25 | 0 0 0 1 | -> We do not use this line (and it has to stay 0,0,0,1)
26
27 METHOD #1: Using a Matrix4f
28 This is the "manual" method, perfect to understand but error prone !
29 */
30 Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
31
32 // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
33 float theta = M_PI / 4; // The angle of rotation in radians
34 transform_1(0, 0) = cos(theta);
35 transform_1(0, 1) = -sin(theta);
36 transform_1(1, 0) = sin(theta);
37 transform_1(1, 1) = cos(theta);
38 transform_1(0, 3) = 20;
39
40 // Print the transformation
41 printf("Method #1: using a Matrix4f\n");
42 std::cout << transform_1 << std::endl;
43
44 /* METHOD #2: Using a Affine3f
45 This method is easier and less error prone
46 */
47 Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
48
49 // Define a translation of 2.5 meters on the x axis.
50 transform_2.translation() << 20.0, 0.0, 0.0;
51
52 // The same rotation matrix as before; theta radians around Z axis
53 transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
54
55 // Print the transformation
56 printf("\nMethod #2: using an Affine3f\n");
57 std::cout << transform_2.matrix() << std::endl;
58
59 // Executing the transformation
60 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
61 // You can either apply transform_1 or transform_2; they are the same
62 pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);
63
64 // Visualization
65 printf("\nPoint cloud colors : white = original point cloud\n"
66 " red = transformed point cloud\n");
67 pcl::visualization::PCLVisualizer viewer("Matrix transformation example");
68
69 // Define R,G,B colors for the point cloud
70 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler(source_cloud, 255, 255, 255);
71 // We add the point cloud to the viewer and pass the color handler
72 viewer.addPointCloud(source_cloud, source_cloud_color_handler, "original_cloud");
73
74 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler(transformed_cloud, 230, 20, 20); // Red
75 viewer.addPointCloud(transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
76
77 viewer.addCoordinateSystem(1.0, "cloud", 0);
78 viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
79 viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
80 viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
81 //viewer.setPosition(800, 400); // Setting visualiser window position
82
83 while (!viewer.wasStopped()) { // Display the visualiser until 'q' key is pressed
84 viewer.spinOnce();
85 }
86 return 0;
87 }
