pcl 使用矩阵变换（旋转、平移）点云
目录
1. 场景
2. 代码详解
2.1 头文件
2.2 参数解析
2.3 解析参数获取pcd和ply文件索引
2.4 加载pcd和ply点云数据
2.5 定义变换矩阵
2.5.1 预备知识
2.5.2 实操
2.6 变换
2.7 可视化
3. 整合代码
1. 场景
定义一个4x4的转换矩阵，去转换点云数据。里面包含的知识点：
（1）加载点云数据（pcd or ply）；
（2）转换点云；
（3）可视化点云数据（设置点云颜色、背景颜色）。
2. 代码详解
2.1 头文件
#include
#include
#include
#include
#include // pcl::console::find_switch
#include // pcl::transformPointCloud
#include // pcl::visualization::PCLVisualizer 2.2 参数解析
通过命令行指定点云文件，如果没有设置，则会打印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;
}
// This is the main function
int
main(int argc, char** argv)
{
// Show help，find_switch：argv命令行中是否有-h，有则运行下面函数
if (pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help")) {
showHelp(argv[0]);
return 0;
}
}2.3 解析参数获取pcd和ply文件索引
// Fetch point cloud filename in arguments | Works with PCD and PLY files
std::vector filenames_idx; // 存储文件名索引的数组
bool file_is_pcd = false;
// 返回文件名索引的数组， filenames[0]是第0个文件名的索引，
// argv[filenames[0]]第0个文件名
filenames_idx = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
if (filenames_idx.size() != 1) { // 不是ply文件
filenames_idx = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
if (filenames_idx.size() != 1) { // 也不是pcd文件
showHelp(argv[0]);
return -1;
}
else {
file_is_pcd = true;
}
} 2.4 加载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_idx[0]], *source_cloud) < 0) { // 默认只有一个文件，该文件名：argv[filenames_idx[0]]
std::cout << "Error loading point cloud " << argv[filenames_idx[0]] << std::endl << std::endl;
showHelp(argv[0]);
return -1;
}
}
else {
if (pcl::io::loadPLYFile(argv[filenames_idx[0]], *source_cloud) < 0) {
std::cout << "Error loading point cloud " << argv[filenames_idx[0]] << std::endl << std::endl;
showHelp(argv[0]);
return -1;
}
} 2.5 定义变换矩阵
4x4的变换矩阵，前3x3控制旋转，最右边一列的前3行控制平移。
2.5.1 预备知识
（1）绕x轴逆时针旋转
角度
（2）绕y轴逆时针旋转角度
（3）绕z轴逆时针旋转角度
（4）平移
点A(a,b,c)和点B(x+a,y+b,z+c)，点A沿着AB平移到B.
2.5.2 实操
（1）手动定义4x4变换矩阵
即绕着z轴旋转角度，再沿着x轴平移2.5。
// 举个例子，说明下转换矩阵是如何工作的。
/* Reminder: how transformation matrices work :
最右边一列的前3行是控制平移，所以x,y,z这三个值会平移数据。
其中，x值控制沿着x轴上的平移
|-------> This column is the translation。平移
| 1 0 0 x | \
| 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left。旋转。全部是1，则没有变化
| 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 !
这个是手动设置的4x4矩阵值，容易理解，但是也容易出错！
*/
// 定义4x4转换矩阵
// 前面3x3控制旋转，最右列控制平移
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); // 4x4的单位矩阵
// 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); // 设置这4个值，可以绕z轴旋转theta角度
transform_1(0, 1) = -sin(theta);
transform_1(1, 0) = sin(theta);
transform_1(1, 1) = std::cos(theta);
// Define a translation of 2.5 meters on the x axis.
transform_1(0, 3) = 2.5; // 沿着x平移2.5
// Print the transformation
printf("Method #1: using a Matrix4f\n");
std::cout << transform_1 << std::endl; 
沿着z轴旋转，导致兔子头朝下（右手法则，rgb颜色轴分别是xyz轴）。
（2）使用Affine3f定义4x4矩阵
因为是通过函数定义，所以此法更简单，且不容易出错
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
// Define a translation of 2.5 meters on the x axis.
// 1，定义平移
transform_2.translation() << 2.5, 0.0, 0.0;
// The same rotation matrix as before; theta radians around Z axis
// 2，定义然后Z旋转
transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); // UnitZ 即z轴两种方法的矩阵值是一样的。
2.6 变换
// 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); 2.7 可视化
// Visualization
printf("\nPoint cloud colors : white = original point cloud\n"
" red = transformed point cloud\n");
pcl::visualization::PCLVisualizer viewer("Matrix transformation example"); // 定义可视化对象
// 1，设置原始点云颜色
// 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");
// 2，设置新点云颜色
pcl::visualization::PointCloudColorHandlerCustom transformed_cloud_color_handler(transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud(transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
// 3，其他设置：坐标、背景颜色、渲染
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();
} 3. 整合代码
#include
#include
#include
#include
#include // pcl::console::find_switch
#include // pcl::transformPointCloud
#include // pcl::visualization::PCLVisualizer
// 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，find_switch：argv命令行中是否有-h，有则运行下面函数
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_idx; // 存储文件名索引的数组
bool file_is_pcd = false;
// 返回文件名索引的数组， filenames[0]是第0个文件名的索引，
// argv[filenames[0]]第0个文件名
filenames_idx = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
if (filenames_idx.size() != 1) { // 不是ply文件
filenames_idx = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
if (filenames_idx.size() != 1) { // 也不是pcd文件
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_idx[0]], *source_cloud) < 0) { // 默认只有一个文件，该文件名：argv[filenames_idx[0]]
std::cout << "Error loading point cloud " << argv[filenames_idx[0]] << std::endl << std::endl;
showHelp(argv[0]);
return -1;
}
}
else {
if (pcl::io::loadPLYFile(argv[filenames_idx[0]], *source_cloud) < 0) {
std::cout << "Error loading point cloud " << argv[filenames_idx[0]] << std::endl << std::endl;
showHelp(argv[0]);
return -1;
}
}
// 举个例子，说明下转换矩阵是如何工作的。
/* Reminder: how transformation matrices work :
最右边一列的前3行是控制平移，所以x,y,z这三个值会平移数据。
其中，x值控制沿着x轴上的平移
|-------> This column is the translation。平移
| 1 0 0 x | \
| 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left。旋转。全部是1，则没有变化
| 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 !
这个是手动设置的4x4矩阵值，容易理解，但是也容易出错！
*/
// 定义4x4转换矩阵
// 前面3x3控制旋转，最右列控制平移
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); // 4x4的单位矩阵
// 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); // 设置这4个值，可以绕z轴旋转theta角度
transform_1(0, 1) = -sin(theta);
transform_1(1, 0) = sin(theta);
transform_1(1, 1) = std::cos(theta);
// Define a translation of 2.5 meters on the x axis.
transform_1(0, 3) = 2.5; // 沿着x平移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.
// 1，定义平移
transform_2.translation() << 2.5, 0.0, 0.0;
// The same rotation matrix as before; theta radians around Z axis
// 2，定义然后Z旋转
transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); // UnitZ 即z轴
// 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"); // 定义可视化对象
// 1，设置原始点云颜色
// 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");
// 2，设置新点云颜色
pcl::visualization::PointCloudColorHandlerCustom transformed_cloud_color_handler(transformed_cloud, 230, 20, 20); // Red
viewer.addPointCloud(transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");
// 3，其他设置：坐标、背景颜色、渲染
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;
} 参考：Using a matrix to transform a point cloud — Point Cloud Library 0.0 documentation