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

 //icpInteractive.cpp
 //函数：main()
 //功能：导入两个视角的点云数据，第一个点云数据作为源，第二个点云数据作为目标，进行icp配准，
 //      并显示配准结果，按空格进行每次迭代的交互
 //输入：导入文件名，输出文件名 例：可执行文件根目录下，命令行输入 xml2pcd file1.pcd file2.pcd
 //创建时间：2014/09/10
 //最近更新时间：2014/09/17
 //创建者：肖泽东

 #include <iostream>

 #include <pcl/io/pcd_io.h>
 #include <pcl/point_cloud.h>
 #include <pcl/console/parse.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/registration/icp.h>

 bool next_iteration = false;

 //显示帮助
 void showHelp(char* program_name)
 {
     std::cout << std::endl;
     std::cout << "Usage: " << program_name << " sourcefile.pcd targetfile.pcd" << std::endl;
     std::cout << "-h: Show this help." << std::endl;
 }

 //键盘交互
 void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,
                        void* nothing)
 {
   if (event.getKeySym () == "space" && event.keyDown ())    //判别是否按下空格按键
     next_iteration = true;    //允许下一次迭代
 }

 int main(int argc, char** argv)
 {
     // 显示帮助文档
     if(pcl::console::find_switch (argc,argv,"-h") || pcl::console::find_switch(argc,argv,"--help"))
     {
         showHelp(argv[]);
         return ;
     }

     std::vector<int> filenames;    //输入参数 文件名向量

     filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");    //查找输入中的pcd文件
     if (filenames.size() != )    //限定输入两个pcd点云文件
     {
         std::cout << "input pcd file format wrong" << std::endl;    //不是两个，显示出错
         showHelp(argv[]);
         return -;
     }
     //定义配准源点云和目标点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());    //源点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ> ());    //目标点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ> ());;    //配准后点云

     if(pcl::io::loadPCDFile(argv[filenames[]],*source_cloud) < )    //导入源点云
     {
         std::cout << "Error loading point cloud " << argv[filenames[]] << std::endl;
         showHelp(argv[]);
         return -;
     }
     if(pcl::io::loadPCDFile(argv[filenames[]],*target_cloud) < )    //导入目标点云
     {
         std::cout << "Error loading point cloud " << argv[filenames[]] << std::endl;
         showHelp(argv[]);
         return -;
     }

     //ICP 配准定义
     pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;    //定义ICP类型实例icp
     *aligned_cloud = *source_cloud;        //初始化配准点云为源点云
     icp.setInputCloud(aligned_cloud);    //初始化源点云
     icp.setInputTarget(target_cloud);    //初始化目标点云
     //设置ICP基本参数
     icp.setMaxCorrespondenceDistance(); //设置对应点容忍最大距离
     icp.setMaximumIterations();    //设置每次按键触发最大迭代次数
     icp.setRANSACIterations();    //不进行RANSAC迭代

     // Visualization显示结果
     printf(  "\nPoint cloud colors :  white  = original point cloud\n"
         "                        red  = transformed point cloud\n");

     pcl::visualization::PCLVisualizer viewer ("ICP test example");    //定义显示窗口
     int v1 ();    //分隔窗口v1
     int v2 ();    //分隔窗口v2
     viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);    //...viewPort(x_min,y_min,x_max,y_max,int ..)
     viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);    //
     //std::cout << "v1=" << v1 << "v2=" << v2<< std::endl;

     // Define R,G,B colors for the point cloud
     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, , , );//White
     // We add the point cloud to the viewer and pass the color handler
     viewer.addPointCloud (source_cloud, source_cloud_color_handler, "source_cloud_v1", v1);

     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_cloud_color_handler (target_cloud, , , ); // Red
     viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v1", v1);

     //viewer.addCoordinateSystem (1.0, "cloud", 0);
     //viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
     viewer.setBackgroundColor(0.05, 0.05, 0.05, v1);
     viewer.setBackgroundColor(0.05, 0.05, 0.05, v2);
     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, , "source_cloud_v1");
     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, , "target_cloud_v1");
     //viewer.setPosition(800, 400); // Setting visualiser window position

     // Define R,G,B colors for the point cloud
     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_cloud_color_handler (aligned_cloud, , , ); //White
     // We add the point cloud to the viewer and pass the color handler
     viewer.addPointCloud (aligned_cloud, aligned_cloud_color_handler, "aligned_cloud_v2", v2);
     viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v2", v2);

     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, , "aligned_cloud_v2");
     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, , "target_cloud_v2");
     //viewer.setPosition(800, 400); // Setting visualiser window position
     // Register keyboard callback :
     viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);    //对齐响应键盘事件

     int iterations = ;    //迭代次数
     while (!viewer.wasStopped ())
     { // Display the visualiser until 'q' key is pressed
         viewer.spinOnce ();    //运行视图
         if(next_iteration)
         {
             icp.align(*aligned_cloud);    //ICP配准对齐结果
             std::cout << "has converged:" << icp.hasConverged() << " score: " <<
                 icp.getFitnessScore() << std::endl;    //配准分析...icp.hasConverge()=1,表示收敛
             std::cout << icp.getFinalTransformation() << std::endl;    //获取变换矩阵
             std::cout << "Iteration = " << ++iterations; //迭代次数加1
             if(iterations ==)    //迭代满100次停止迭代
                 return ;
             viewer.updatePointCloud (aligned_cloud, aligned_cloud_color_handler, "aligned_cloud_v2");    //更新点云
         }
         next_iteration = false;    //本次迭代结束，不直接进行下一次迭代，等待下次迭代触发
     }
     return ;
 }

　　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})