PCL学习笔记->基本用法4(使用一个矩阵转换点云)
前言
由于我的系统是Ubuntu16.04，IDE是qtcreator（没有使用qmake，用的是cmake），所以官网教程中基础用法的一些东西并不是特别的适合我（其实是因为懒），因而就不再翻译了。列表如下～
Compiling PCL from source on POSIX compliant systems
Customizing the PCL build process
Building PCL’s dependencies from source on Windows
Compiling PCL from source on Windows
Compiling PCL and its dependencies from MacPorts and source on Mac OS X
Installing on Mac OS X using Homebrew
Using PCL with Eclipse
Generate a local documentation for PCL
应用一个矩阵转化点云
在本教程中，我们将学习如何应用一个4×4的矩阵转换点云数据。我们对点云的原始数据进行了平移和旋转，最后展示了结果。
这个程序加载PCL或者PLY文件，然后使用一个矩阵对它进行变换，最后展示了最初和变换后的点云。
程序
首先，创建一个文件，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 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::Ptr source_cloud (new pcl::PointCloud ());
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) = cos (theta);
transform_1 (0,1) = -sin(theta);
transform_1 (1,0) = sin (theta);
transform_1 (1,1) = 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::Ptr transformed_cloud (new pcl::PointCloud ());
// 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 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 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;
}
解释
头文件如下
#include
#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;
}如果我们没有提供期望的参数，这个函数将输出帮助信息。
// Show help
if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) {
showHelp (argv[0]);
return 0;
}解析命令行参数，使用“-h”或者”--help“将打印帮助信息。同时也将会终止程序。
// Fetch point cloud filename in arguments | Works with PCD and PLY files
std::vector 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;
}
} 在参数中找到.ply或者.pcd文件名。如果没有发现，终止程序。file_is_pcd将帮助我们选择加载PCD或PLY文件。
// Load file | Works with PCD and PLY files
pcl::PointCloud::Ptr source_cloud (new pcl::PointCloud ());
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;
}
} 读取PCD/PLY文件，并且检查是否被正确读取，没有的话就终止程序。
/* 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();
这是第一种方法创建转换。这是为了帮助你明白欧式变换矩阵如何工作的。我们初始化这个4×4的矩阵为单位矩阵。
| 1 0 0 0 |
i = | 0 1 0 0 |
| 0 0 1 0 |
| 0 0 0 1 |
注意：单位矩阵等于1，当它乘一个数字的时候，什么都没有发生。单位矩阵是正方形，对角线全为1，而其他地方为0。
这意味着没有发生旋转和平移，最后一行没有什么用，只是一个齐次坐标。
左侧的3×3矩阵是旋转矩阵，右侧的3×1的矩阵是平移矩阵。
// 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) = cos (theta);
transform_1 (0,1) = -sin(theta);
transform_1 (1,0) = sin (theta);
transform_1 (1,1) = 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;
这里我们定义了一个绕Z轴的45°旋转，和沿着X轴线的平移。我们定义的变换矩阵如下：
| 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;第二个方法更简单并且出错的可能性较小。需要注意的是旋转不可互换，这意味着: rotA * rotB != rotB * rotA。
// Executing the transformation
pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ());
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);
对source_cloud 进行变换，且结果保留在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 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 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 ();
}
使用 PCLVisualizer查看结果。原始点云是白色的，而变换后的点云是红色的。三个轴也会显示。我们也设置了visualizer背景的颜色和显示点云的大小。
编译程序
在CMakeLists.txt中添加如下内容：
cmake_minimum_required(VERSION 2.6 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})
得到可执行文件虎，运行并且跟上PCD或PLY文件。为了重现结果，你可以下载这个cube.ply文件。
$ ./matrix_transform cube.ply
结果类似如下：
./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
关于转换
现在我们已经成功的进行了转换。但当我们想要对一个点进行转换怎么办？A vector ?
一个点在3D空间中包括了3个坐标 x,y,z (笛卡尔坐标系)。
怎么让一个向量乘一个4×4的矩阵。
我们需要一个有4个元素的向量。但最后一个元素怎么处理，它取决于你想要怎么做：
- 如果你想要变换一个点且最后一个元素设置为1，这样添加了平移。
- 如果你想要变换一个点且最后一个元素设置为0，这样忽视了平移。
[10, 5, 0, 3, 0, -1]这是一个简单的例子，我们想要变换如下的向量。前3个数据定义了坐标，后3个数据定义了方向。
这个向量起始于点10, 5, 0；终止于13, 5, -1。
你想要的进行的变换如下所示：
[10, 5, 0, 1] * 4x4_transformation_matrix
[3, 0, -1, 0] * 4x4_transformation_matrix