Using a matrix to transform a point cloud

In this tutorial we will learn how to transform a point cloud using a 4x4 matrix. We will apply a rotation and a translation to a loaded point cloud and display the result.

This program is able to load one PCD or PLY file; apply a matrix transformation on it and display the original and transformed point cloud.

The code

First, create a file, let’s say, matrix_transform.cpp in your favorite editor, and place the following code inside it:

  1#include <iostream>
  2
  3#include <pcl/io/pcd_io.h>
  4#include <pcl/io/ply_io.h>
  5#include <pcl/point_cloud.h>
  6#include <pcl/console/parse.h>
  7#include <pcl/common/transforms.h>
  8#include <pcl/visualization/pcl_visualizer.h>
  9
 10// This function displays the help
 11void
 12showHelp(char * program_name)
 13{
 14  std::cout << std::endl;
 15  std::cout << "Usage: " << program_name << " cloud_filename.[pcd|ply]" << std::endl;
 16  std::cout << "-h:  Show this help." << std::endl;
 17}
 18
 19// This is the main function
 20int
 21main (int argc, char** argv)
 22{
 23
 24  // Show help
 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;
 32  bool file_is_pcd = false;
 33
 34  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".ply");
 35
 36  if (filenames.size () != 1)  {
 37    filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
 38
 39    if (filenames.size () != 1) {
 40      showHelp (argv[0]);
 41      return -1;
 42    } else {
 43      file_is_pcd = true;
 44    }
 45  }
 46
 47  // Load file | Works with PCD and PLY files
 48  pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
 49
 50  if (file_is_pcd) {
 51    if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0)  {
 52      std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
 53      showHelp (argv[0]);
 54      return -1;
 55    }
 56  } else {
 57    if (pcl::io::loadPLYFile (argv[filenames[0]], *source_cloud) < 0)  {
 58      std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
 59      showHelp (argv[0]);
 60      return -1;
 61    }
 62  }
 63
 64  /* Reminder: how transformation matrices work :
 65
 66           |-------> This column is the translation
 67    | 1 0 0 x |  \
 68    | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
 69    | 0 0 1 z |  /
 70    | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)
 71
 72    METHOD #1: Using a Matrix4f
 73    This is the "manual" method, perfect to understand but error prone !
 74  */
 75  Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
 76
 77  // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
 78  float theta = M_PI/4; // The angle of rotation in radians
 79  transform_1 (0,0) = std::cos (theta);
 80  transform_1 (0,1) = -sin(theta);
 81  transform_1 (1,0) = sin (theta);
 82  transform_1 (1,1) = std::cos (theta);
 83  //    (row, column)
 84
 85  // Define a translation of 2.5 meters on the x axis.
 86  transform_1 (0,3) = 2.5;
 87
 88  // Print the transformation
 89  printf ("Method #1: using a Matrix4f\n");
 90  std::cout << transform_1 << std::endl;
 91
 92  /*  METHOD #2: Using a Affine3f
 93    This method is easier and less error prone
 94  */
 95  Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
 96
 97  // Define a translation of 2.5 meters on the x axis.
 98  transform_2.translation() << 2.5, 0.0, 0.0;
 99
100  // The same rotation matrix as before; theta radians around Z axis
101  transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
102
103  // Print the transformation
104  printf ("\nMethod #2: using an Affine3f\n");
105  std::cout << transform_2.matrix() << std::endl;
106
107  // Executing the transformation
108  pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
109  // You can either apply transform_1 or transform_2; they are the same
110  pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);
111
112  // Visualization
113  printf(  "\nPoint cloud colors :  white  = original point cloud\n"
114      "                        red  = transformed point cloud\n");
115  pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
116
117   // Define R,G,B colors for the point cloud
118  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
119  // We add the point cloud to the viewer and pass the color handler
120  viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");
121
122  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
123  viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
124
125  viewer.addCoordinateSystem (1.0, "cloud", 0);
126  viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
127  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
128  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
129  //viewer.setPosition(800, 400); // Setting visualiser window position
130
131  while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
132    viewer.spinOnce ();
133  }
134
135  return 0;
136}

The explanation

Now, let’s break down the code piece by piece.

#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>

We include all the headers we will make use of. #include <pcl/common/transforms.h> allows us to use pcl::transformPointCloud function.

// 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 function display the help in case the user didn’t provide expected arguments.

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

We parse the arguments on the command line, either using -h or –help will display the help. This terminates the program

  // 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;
    }
  }

We look for .ply or .pcd filenames in the arguments. If not found; terminate the program. The bool file_is_pcd will help us choose between loading PCD or PLY file.

  // 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;
    }
  }

We now load the PCD/PLY file and check if the file was loaded successfully. Otherwise terminate the program.

  /* 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();

This is a first approach to create a transformation. This will help you understand how transformation matrices work. We initialize a 4x4 matrix to identity;

    |  1  0  0  0  |
i = |  0  1  0  0  |
    |  0  0  1  0  |
    |  0  0  0  1  |

Note

The identity matrix is the equivalent of “1” when multiplying numbers; it changes nothing. It is a square matrix with ones on the main diagonal and zeros elsewhere.

This means no transformation (no rotation and no translation). We do not use the last row of the matrix.

The first 3 rows and columns (top left) components are the rotation matrix. The first 3 rows of the last column is the translation.

  // 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;

Here we defined a 45° (PI/4) rotation around the Z axis and a translation on the X axis. This is the transformation we just defined

    |  cos(θ) -sin(θ)  0.0 |
R = |  sin(θ)  cos(θ)  0.0 |
    |  0.0     0.0     1.0 |

t = < 2.5, 0.0, 0.0 >

  /*  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;

This second approach is easier to understand and is less error prone. Be careful if you want to apply several rotations; rotations are not commutative ! This means than in most cases: rotA * rotB != rotB * rotA.

  // 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);

Now we apply this matrix on the point cloud source_cloud and we save the result in the newly created transformed_cloud.

  // 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;

We then visualize the result using the PCLVisualizer. The original point cloud will be displayed white and the transformed one in red. The coordoniates axis will be displayed. We also set the background color of the visualizer and the point display size.

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 2
 3project(pcl-matrix_transform)
 4
 5find_package(PCL 1.7 REQUIRED)
 6
 7include_directories(${PCL_INCLUDE_DIRS})
 8link_directories(${PCL_LIBRARY_DIRS})
 9add_definitions(${PCL_DEFINITIONS})
10
11add_executable (matrix_transform matrix_transform.cpp)
12target_link_libraries (matrix_transform ${PCL_LIBRARIES})

After you have made the executable, run it passing a path to a PCD or PLY file. To reproduce the results shown below, you can download the cube.ply file:

$ ./matrix_transform cube.ply

You will see something similar to this:

./matrix_transform cube.ply
[pcl::PLYReader] /home/victor/cube.ply:12: property 'list uint8 uint32 vertex_indices' of element 'face' is not handled
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       2.5
 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
_images/cube_big.png

More about transformations

So now you successfully transformed a point cloud using a transformation matrix.
What if you want to transform a single point ? A vector ?
A point is defined in 3D space with its three coordinates; x,y,z (in a cartesian coordinate system).
How can you multiply a vector (with 3 coordinates) with a 4x4 matrix ? You simply can’t ! If you don’t know why please refer to matrix multiplications on wikipedia.

We need a vector with 4 components. What do you put in the last component ? It depends on what you want to do:

  • If you want to transform a point: put 1 at the end of the vector so that the translation is taken in account.

  • If you want to transform the direction of a vector: put 0 at the end of the vector to ignore the translation.

Here’s a quick example, we want to transform the following vector:

[10, 5, 0, 3, 0, -1]
Where the first 3 components defines the origin coordinates and the last 3 components the direction.
This vector starts at point 10, 5, 0 and ends at 13, 5, -1.

This is what you need to do to transform the vector:

[10, 5, 0,  1] * 4x4_transformation_matrix
[3,  0, -1, 0] * 4x4_transformation_matrix