#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>//用于显示帮助信息的函数voidshowHelp(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;}// 主函数intmain(int argc,char**argv){
//pcl::console::find_switch (int argc, char** argv, const char* argument_name)// * \param[in] argc the number of command line arguments 命令行参数个数// * \param[in] argv the command line arguments 命令行参数// * \param[in] argument_name the string value to search for 需要查找的命令行参数名称// * \return index of found argument or -1 if arguments do not appear in list if(pcl::console::find_switch(argc, argv,"-h")|| pcl::console::find_switch(argc, argv,"--help")){
//argv[0]是命令行参数中的第一个，即程序名showHelp(argv[0]);return0;}// Fetch point cloud filename in arguments | Works with PCD and PLY files//创建用于存储filename indices的vector,这里所谓的filename indices其实是文件名在命令行参数中的索引
std::vector<int> filenames;//文件类型标识符 pcd文件为true ply文件为flasebool file_is_pcd =false;// pcl::console::parse_file_extension_argument (int argc, char** argv, const std::string &ext);// /** \brief Parse command line arguments for file names with given extension// * \param[in] argc the number of command line arguments// * \param[in] argv the command line arguments// * \param[in] ext the extension to search for// * \return a vector with file names indices//查找后缀名为.ply的文件，返回file names indices,返回的应该是后缀名为.ply的文件在命令行参数中的索引
filenames = pcl::console::parse_file_extension_argument(argc, argv,".ply");//如果filenames.size()不等于1，认为没有找到.ply类型的文件if(filenames.size()!=1){
//转而查找后缀名为.pcd的文件
filenames = pcl::console::parse_file_extension_argument(argc, argv,".pcd");//如果仍然没找到，就调用显示帮助信息的函数，并return -1,表示程序运行出错if(filenames.size()!=1){
showHelp(argv[0]);return-1;}//否则表明找到.pcd文件，将原来默认为flase的file_is_pcd更改为trueelse{
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){
//pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud//* * < 0 (-1) on error//* * == 0 on successif(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
|100 x | \
|010 y |}-> The identity 3x3 matrix (no rotation) on the left
|001 z |/|0001|-> We donot use this line (and it has to stay 0,0,0,1)//左上角3x3是旋转矩阵，最右侧前3行是平移，最后一行不使用，但是必须为(0,0,0,1)
METHOD
#1: Using a Matrix4f ====================================第二种方法
This is the "manual" method, perfect to understand but error prone !*/// 创建4x4单位阵
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 radianstransform_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 transformationprintf("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 transformationprintf("\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 positionwhile(!viewer.wasStopped()){
// Display the visualiser until 'q' key is pressed
viewer.spinOnce();}return0;}