第二周：01 ICP迭代交互
本周主要任务01：利用PCL库函数，ICP融合两个角度的点云
任务时间：2014年9月8日-2014年9月14日
任务完成情况：可以使用键盘交互，显示每次ICP迭代结果
任务涉及基本方法：
1.PCL库中ICP相关函数，ICP交互迭代 参考官方教程 http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp
程序文件：
1.icpInteractive.cpp
1 //icpInteractive.cpp 2 //函数：main() 3 //功能：导入两个视角的点云数据，第一个点云数据作为源，第二个点云数据作为目标，进行icp配准， 4 // 并显示配准结果，按空格进行每次迭代的交互 5 //输入：导入文件名，输出文件名 例：可执行文件根目录下，命令行输入 xml2pcd file1.pcd file2.pcd 6 //创建时间：2014/09/10 7 //最近更新时间：2014/09/17 8 //创建者：肖泽东 9 10 #include <iostream> 11 12 #include <pcl/io/pcd_io.h> 13 #include <pcl/point_cloud.h> 14 #include <pcl/console/parse.h> 15 #include <pcl/visualization/pcl_visualizer.h> 16 #include <pcl/registration/icp.h> 17 18 bool next_iteration = false; 19 20 //显示帮助 21 void showHelp(char* program_name) 22 { 23 std::cout << std::endl; 24 std::cout << "Usage: " << program_name << " sourcefile.pcd targetfile.pcd" << std::endl; 25 std::cout << "-h: Show this help." << std::endl; 26 } 27 28 //键盘交互 29 void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event, 30 void* nothing) 31 { 32 if (event.getKeySym () == "space" && event.keyDown ()) //判别是否按下空格按键 33 next_iteration = true; //允许下一次迭代 34 } 35 36 int main(int argc, char** argv) 37 { 38 // 显示帮助文档 39 if(pcl::console::find_switch (argc,argv,"-h") || pcl::console::find_switch(argc,argv,"--help")) 40 { 41 showHelp(argv[0]); 42 return 0; 43 } 44 45 std::vector<int> filenames; //输入参数 文件名向量 46 47 filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd"); //查找输入中的pcd文件 48 if (filenames.size() != 2) //限定输入两个pcd点云文件 49 { 50 std::cout << "input pcd file format wrong" << std::endl; //不是两个，显示出错 51 showHelp(argv[0]); 52 return -1; 53 } 54 //定义配准源点云和目标点云 55 pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); //源点云 56 pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); //目标点云 57 pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ> ());; //配准后点云 58 59 if(pcl::io::loadPCDFile(argv[filenames[0]],*source_cloud) < 0) //导入源点云 60 { 61 std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl; 62 showHelp(argv[0]); 63 return -1; 64 } 65 if(pcl::io::loadPCDFile(argv[filenames[1]],*target_cloud) < 0) //导入目标点云 66 { 67 std::cout << "Error loading point cloud " << argv[filenames[1]] << std::endl; 68 showHelp(argv[0]); 69 return -1; 70 } 71 72 //ICP 配准定义 73 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; //定义ICP类型实例icp 74 *aligned_cloud = *source_cloud; //初始化配准点云为源点云 75 icp.setInputCloud(aligned_cloud); //初始化源点云 76 icp.setInputTarget(target_cloud); //初始化目标点云 77 //设置ICP基本参数 78 icp.setMaxCorrespondenceDistance(100); //设置对应点容忍最大距离 79 icp.setMaximumIterations(1); //设置每次按键触发最大迭代次数 80 icp.setRANSACIterations(0); //不进行RANSAC迭代 81 82 // Visualization显示结果 83 printf( "\nPoint cloud colors : white = original point cloud\n" 84 " red = transformed point cloud\n"); 85 86 87 pcl::visualization::PCLVisualizer viewer ("ICP test example"); //定义显示窗口 88 int v1 (0); //分隔窗口v1 89 int v2 (1); //分隔窗口v2 90 viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1); //...viewPort(x_min,y_min,x_max,y_max,int ..) 91 viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2); // 92 //std::cout << "v1=" << v1 << "v2=" << v2<< std::endl; 93 94 // Define R,G,B colors for the point cloud 95 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);//White 96 // We add the point cloud to the viewer and pass the color handler 97 viewer.addPointCloud (source_cloud, source_cloud_color_handler, "source_cloud_v1", v1); 98 99 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_cloud_color_handler (target_cloud, 230, 20, 20); // Red 100 viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v1", v1); 101 102 //viewer.addCoordinateSystem (1.0, "cloud", 0); 103 //viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey 104 viewer.setBackgroundColor(0.05, 0.05, 0.05, v1); 105 viewer.setBackgroundColor(0.05, 0.05, 0.05, v2); 106 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source_cloud_v1"); 107 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target_cloud_v1"); 108 //viewer.setPosition(800, 400); // Setting visualiser window position 109 110 // Define R,G,B colors for the point cloud 111 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_cloud_color_handler (aligned_cloud, 255, 255, 255); //White 112 // We add the point cloud to the viewer and pass the color handler 113 viewer.addPointCloud (aligned_cloud, aligned_cloud_color_handler, "aligned_cloud_v2", v2); 114 viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v2", v2); 115 116 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "aligned_cloud_v2"); 117 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target_cloud_v2"); 118 //viewer.setPosition(800, 400); // Setting visualiser window position 119 // Register keyboard callback : 120 viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL); //对齐响应键盘事件 121 122 int iterations = 0; //迭代次数 123 while (!viewer.wasStopped ()) 124 { // Display the visualiser until 'q' key is pressed 125 viewer.spinOnce (); //运行视图 126 if(next_iteration) 127 { 128 icp.align(*aligned_cloud); //ICP配准对齐结果 129 std::cout << "has converged:" << icp.hasConverged() << " score: " << 130 icp.getFitnessScore() << std::endl; //配准分析...icp.hasConverge()=1,表示收敛 131 std::cout << icp.getFinalTransformation() << std::endl; //获取变换矩阵 132 std::cout << "Iteration = " << ++iterations; //迭代次数加1 133 if(iterations ==100) //迭代满100次停止迭代 134 return 0; 135 viewer.updatePointCloud (aligned_cloud, aligned_cloud_color_handler, "aligned_cloud_v2"); //更新点云 136 } 137 next_iteration = false; //本次迭代结束，不直接进行下一次迭代，等待下次迭代触发 138 } 139 return 0; 140 }
2.CMakeLists.txt
cmake_minimum_required(VERSION 2.6 FATAL_ERROR) project(pcl_icpInteractive) find_package(PCL 1.6 REQUIRED) include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) add_executable (icpInteractive icpInteractive.cpp) target_link_libraries (icpInteractive ${PCL_LIBRARIES})
