第二周：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})

 

转载于:https://www.cnblogs.com/xzd1575/p/3977960.html