第二周02：Fusion ICP逐帧融合

本周主要任务02：Fusion 使用ICP进行逐帧融合

任务时间： 2014年9月8日-2014年9月14日

任务完成情况：

　　已实现将各帧融合到统一的第一帧所定义的摄像机坐标系下，但是由于部分帧之间的ICP融合结果　　不佳，导致所有帧融合在统一坐标系下结果不好。

任务涉及基本方法：

　　1.exe文件当前目录搜索文件

程序文件：

fusion.cpp

 //fusion.cpp
 //函数：main()
 //功能：
 //输入：
 //创建时间：2014/09/10
 //最近更新时间：2014/09/16
 //创建者：肖泽东

 #include <iostream>
 #include <fstream>

 #include <pcl/io/pcd_io.h>
 #include <pcl/io/io.h>
 #include <pcl/point_cloud.h>
 #include <pcl/console/parse.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/registration/icp.h>
 int state = ;
 bool next_flag = 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 ())    //空格按键触发
     {
         state++;    //状态标志
         state = state % ;    //四种状态依次交替
         next_flag = 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 ;
     }
     //搜索.xml文件名
     fstream outFile;
     char buffer[];
     //std::string dir = "C:\\Users\\xzd\\Documents\\KinectFile\\2014-09-07\\Select\\mengyue\\";
     std::string dir = ".\\pcdFiles\\";    //在exe当前目录搜索文件夹(.\\...)
     std::string source_filename, target_filename;    //定义ICP融合源文件和目标文件
     outFile.open("pcdFileList.txt",ios::in);    //打开保存各帧pcd文件的文件列表txt

     Eigen::Matrix4f RT[];    //定义矩阵数组保存各帧之间的坐标系变换矩阵
     Eigen::Matrix4f RT2First[];    //定义矩阵数组保存各帧到起始帧之间的坐标系变换矩阵
     int index = ;    //定义帧索引

     //定义配准源点云和目标点云
     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> ());    //配准对齐后点云

     //ICP 配准定义
     pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;    //定义ICP类型实例icp
     //设置ICP基本参数
     icp.setMaxCorrespondenceDistance(); //设置对应点容忍最大距离
     icp.setMaximumIterations();    //设置最大迭代次数
     icp.setRANSACIterations();    //不进行RANSAC迭代

     outFile.getline(buffer,,'\n');    //读取第一行,将该行数据存到buffer
     target_filename = dir + buffer;    //目标点云位置，第一帧点云为最初的目标点云
     std::cout << target_filename << std::endl;
     if(pcl::io::loadPCDFile(target_filename,*target_cloud) < )    //导入目标点云
     {
         std::cout << "Error loading point cloud " << target_filename << std::endl;
         showHelp(argv[]);
         return -;
     }

     while(index < )    //加入计算的帧数
     {
         outFile.getline(buffer,,'\n');    //从第二行起，依次获取每行数据
         source_filename = dir + buffer;    //源点云位置，除第一帧点云外，其他帧依次作为其前一帧的ICP源点云
         std::cout << source_filename << std::endl;    //

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

         icp.setInputCloud(source_cloud);    //初始化源点云
         icp.setInputTarget(target_cloud);    //初始化目标点云

         icp.align(*aligned_cloud);    //配准后点云
         std::cout << "has converged:" << icp.hasConverged() << " score: " <<
             icp.getFitnessScore() << std::endl;    //配准结果

         RT[index] = icp.getFinalTransformation();    //将得到的ICP变换矩阵赋值给RT矩阵数组,
             //由index索引，RT[0]表示第二帧点云向第一帧点云的变换矩阵

         //计算所有点云向第一帧点云的变换，计算方法是相邻帧点云变换矩阵的累乘
         if(index == )    //如果第一个变换
             RT2First[index] = RT[index];    //第二帧先第一帧的变换
         else
             RT2First[index] = RT[index] * RT2First[index-];    //其他帧向第一帧的变换

         index++;    //转向下一变换

         std::cout << icp.getFinalTransformation() << std::endl;    //输出变换矩阵
         *target_cloud = *source_cloud;    //当前帧作为目标点云，下一次循环将使下一帧点云作为源点云
     }
     outFile.close();    //结束关闭pcd文件列表txt
     std::cout << "Computer transform matrix completed" << std::endl;    //提示变换矩阵计算完成信息

     //测试各帧到第一帧的变换是否准确
     target_filename = dir + "Depth0070.xml.pcd";    //目标帧：第一帧
     source_filename = dir + "Depth0071.xml.pcd";    //源帧：可以是已计算的系列帧中任意一帧

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

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

     //执行变换
     pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());    //定义变换后点云
     pcl::PointCloud<pcl::PointXYZ>& Transform_cloud = *transformed_cloud;    //点云

     pcl::transformPointCloud (*source_cloud, *transformed_cloud,RT2First[]);    //执行变换
     icp.setInputCloud(transformed_cloud);    //初始化源点云
     icp.setInputTarget(target_cloud);    //初始化目标点云
     //icp.align(*aligned_cloud);    //执行融合，ICP

     pcl::visualization::PCLVisualizer viewer ("ICP transform");    //定义显示对象实例

     int v1 ();    //显示窗口分隔
     int v2 ();
     viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);    //窗口分隔位置 creatViewPort(x_min,y_min,x_max,y_max,int &viewport)
     viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);

     // 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> transformed_cloud_color_handler (transformed_cloud, , , ); //White
     // We add the point cloud to the viewer and pass the color handler
     viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud_v2", v2);
     viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v2", v2);
     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, , "transformed_cloud_v2");
     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, , "target_cloud_v2");

     viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);    //触发回调函数，查询按键

     while(!viewer.wasStopped())
     {
         viewer.spinOnce();    //窗口刷新
         if(next_flag)
         {
             //std::cout << state << std::endl;
             switch(state)
             {
             case :    //状态1：只显示目标点云
                 viewer.removePointCloud("transformed_cloud_v2",v2);
                 std::cout << "Target Point Cloud" << std::endl;
                 break;
             case :    //状态2：将变换后点云加入到目标点云坐标系
                 viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud_v2", v2);
                 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, , "transformed_cloud_v2");
                 std::cout << "All Point Cloud" << std::endl;
                 break;
             case :    //状态3：移除目标点云，只显示变换后点云
                 viewer.removePointCloud("target_cloud_v2",v2);
                 std::cout << "Transformed Point Cloud" << std::endl;
                 break;
             case :    //状态0：全部显示
                 viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v2", v2);
                 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, , "target_cloud_v2");
                 std::cout << "All Point Cloud" << std::endl;
                 break;
             default:
                 break;
             }
             next_flag = false;    //禁止进行下一次变换
         }
     }
     return ;
 }

CMakeLists.txt

 cmake_minimum_required(VERSION 2.6 FATAL_ERROR)

 project(Fusion_Project)

 find_package(PCL 1.6 REQUIRED)

 include_directories(${PCL_INCLUDE_DIRS})
 link_directories(${PCL_LIBRARY_DIRS})
 add_definitions(${PCL_DEFINITIONS})

 add_executable (fusion fusion.cpp)
 target_link_libraries (fusion ${PCL_LIBRARIES})