第二周02：Fusion ICP逐帧融合
本周主要任务02：Fusion 使用ICP进行逐帧融合
任务时间： 2014年9月8日-2014年9月14日
任务完成情况：
已实现将各帧融合到统一的第一帧所定义的摄像机坐标系下，但是由于部分帧之间的ICP融合结果 不佳，导致所有帧融合在统一坐标系下结果不好。
任务涉及基本方法：
1.exe文件当前目录搜索文件
程序文件：
fusion.cpp
1 //fusion.cpp 2 //函数：main() 3 //功能： 4 //输入： 5 //创建时间：2014/09/10 6 //最近更新时间：2014/09/16 7 //创建者：肖泽东 8 9 #include <iostream> 10 #include <fstream> 11 12 #include <pcl/io/pcd_io.h> 13 #include <pcl/io/io.h> 14 #include <pcl/point_cloud.h> 15 #include <pcl/console/parse.h> 16 #include <pcl/visualization/pcl_visualizer.h> 17 #include <pcl/registration/icp.h> 18 int state = 0; 19 bool next_flag = false; 20 21 //显示帮助 22 void showHelp(char* program_name) 23 { 24 std::cout << std::endl; 25 std::cout << "Usage: " << program_name << " sourcefile.pcd targetfile.pcd" << std::endl; 26 std::cout << "-h: Show this help." << std::endl; 27 } 28 29 //按键触发 30 void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event, 31 void* nothing) 32 { 33 if (event.getKeySym () == "space" && event.keyDown ()) //空格按键触发 34 { 35 state++; //状态标志 36 state = state % 4; //四种状态依次交替 37 next_flag = true; //允许变换到下一状态 38 } 39 } 40 41 int main(int argc, char** argv) 42 { 43 // 显示帮助文档 44 if(pcl::console::find_switch (argc,argv,"-h") || pcl::console::find_switch(argc,argv,"--help")) 45 { 46 showHelp(argv[0]); 47 return 0; 48 } 49 //搜索.xml文件名 50 fstream outFile; 51 char buffer[256]; 52 //std::string dir = "C:\\Users\\xzd\\Documents\\KinectFile\\2014-09-07\\Select\\mengyue\\"; 53 std::string dir = ".\\pcdFiles\\"; //在exe当前目录搜索文件夹(.\\...) 54 std::string source_filename, target_filename; //定义ICP融合源文件和目标文件 55 outFile.open("pcdFileList.txt",ios::in); //打开保存各帧pcd文件的文件列表txt 56 57 Eigen::Matrix4f RT[100]; //定义矩阵数组保存各帧之间的坐标系变换矩阵 58 Eigen::Matrix4f RT2First[100]; //定义矩阵数组保存各帧到起始帧之间的坐标系变换矩阵 59 int index = 0; //定义帧索引 60 61 //定义配准源点云和目标点云 62 pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); //源点云 63 pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); //目标点云 64 pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ> ()); //配准对齐后点云 65 66 //ICP 配准定义 67 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; //定义ICP类型实例icp 68 //设置ICP基本参数 69 icp.setMaxCorrespondenceDistance(100); //设置对应点容忍最大距离 70 icp.setMaximumIterations(20); //设置最大迭代次数 71 icp.setRANSACIterations(0); //不进行RANSAC迭代 72 73 outFile.getline(buffer,256,'\n'); //读取第一行,将该行数据存到buffer 74 target_filename = dir + buffer; //目标点云位置，第一帧点云为最初的目标点云 75 std::cout << target_filename << std::endl; 76 if(pcl::io::loadPCDFile(target_filename,*target_cloud) < 0) //导入目标点云 77 { 78 std::cout << "Error loading point cloud " << target_filename << std::endl; 79 showHelp(argv[0]); 80 return -1; 81 } 82 83 while(index < 20) //加入计算的帧数 84 { 85 outFile.getline(buffer,256,'\n'); //从第二行起，依次获取每行数据 86 source_filename = dir + buffer; //源点云位置，除第一帧点云外，其他帧依次作为其前一帧的ICP源点云 87 std::cout << source_filename << std::endl; // 88 89 if(pcl::io::loadPCDFile(source_filename,*source_cloud) < 0) //导入源点云 90 { 91 std::cout << "Error loading point cloud " << source_filename << std::endl; 92 showHelp(argv[0]); 93 return -1; 94 } 95 96 icp.setInputCloud(source_cloud); //初始化源点云 97 icp.setInputTarget(target_cloud); //初始化目标点云 98 99 icp.align(*aligned_cloud); //配准后点云 100 std::cout << "has converged:" << icp.hasConverged() << " score: " << 101 icp.getFitnessScore() << std::endl; //配准结果 102 103 RT[index] = icp.getFinalTransformation(); //将得到的ICP变换矩阵赋值给RT矩阵数组, 104 //由index索引，RT[0]表示第二帧点云向第一帧点云的变换矩阵 105 106 //计算所有点云向第一帧点云的变换，计算方法是相邻帧点云变换矩阵的累乘 107 if(index == 0) //如果第一个变换 108 RT2First[index] = RT[index]; //第二帧先第一帧的变换 109 else 110 RT2First[index] = RT[index] * RT2First[index-1]; //其他帧向第一帧的变换 111 112 index++; //转向下一变换 113 114 std::cout << icp.getFinalTransformation() << std::endl; //输出变换矩阵 115 *target_cloud = *source_cloud; //当前帧作为目标点云，下一次循环将使下一帧点云作为源点云 116 } 117 outFile.close(); //结束关闭pcd文件列表txt 118 std::cout << "Computer transform matrix completed" << std::endl; //提示变换矩阵计算完成信息 119 120 //测试各帧到第一帧的变换是否准确 121 target_filename = dir + "Depth0070.xml.pcd"; //目标帧：第一帧 122 source_filename = dir + "Depth0071.xml.pcd"; //源帧：可以是已计算的系列帧中任意一帧 123 124 if(pcl::io::loadPCDFile(source_filename,*source_cloud) < 0) //导入源点云 125 { 126 std::cout << "Error loading point cloud " << source_filename << std::endl; 127 showHelp(argv[0]); 128 return -1; 129 } 130 131 if(pcl::io::loadPCDFile(target_filename,*target_cloud) < 0) //导入目标点云 132 { 133 std::cout << "Error loading point cloud " << target_filename << std::endl; 134 showHelp(argv[0]); 135 return -1; 136 } 137 138 //执行变换 139 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); //定义变换后点云 140 pcl::PointCloud<pcl::PointXYZ>& Transform_cloud = *transformed_cloud; //点云 141 142 pcl::transformPointCloud (*source_cloud, *transformed_cloud,RT2First[0]); //执行变换 143 icp.setInputCloud(transformed_cloud); //初始化源点云 144 icp.setInputTarget(target_cloud); //初始化目标点云 145 //icp.align(*aligned_cloud); //执行融合，ICP 146 147 pcl::visualization::PCLVisualizer viewer ("ICP transform"); //定义显示对象实例 148 149 int v1 (0); //显示窗口分隔 150 int v2 (1); 151 viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1); //窗口分隔位置 creatViewPort(x_min,y_min,x_max,y_max,int &viewport) 152 viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2); 153 154 // Define R,G,B colors for the point cloud 155 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);//White 156 // We add the point cloud to the viewer and pass the color handler 157 viewer.addPointCloud (source_cloud, source_cloud_color_handler, "source_cloud_v1", v1); 158 159 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_cloud_color_handler (target_cloud, 230, 20, 20); // Red 160 viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v1", v1); 161 162 //viewer.addCoordinateSystem (1.0, "cloud", 0); 163 //viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey 164 viewer.setBackgroundColor(0.05, 0.05, 0.05, v1); 165 viewer.setBackgroundColor(0.05, 0.05, 0.05, v2); 166 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source_cloud_v1"); 167 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target_cloud_v1"); 168 //viewer.setPosition(800, 400); // Setting visualiser window position 169 170 // Define R,G,B colors for the point cloud 171 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 255, 255, 255); //White 172 // We add the point cloud to the viewer and pass the color handler 173 viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud_v2", v2); 174 viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v2", v2); 175 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud_v2"); 176 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target_cloud_v2"); 177 178 viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL); //触发回调函数，查询按键 179 180 while(!viewer.wasStopped()) 181 { 182 viewer.spinOnce(); //窗口刷新 183 if(next_flag) 184 { 185 //std::cout << state << std::endl; 186 switch(state) 187 { 188 case 1: //状态1：只显示目标点云 189 viewer.removePointCloud("transformed_cloud_v2",v2); 190 std::cout << "Target Point Cloud" << std::endl; 191 break; 192 case 2: //状态2：将变换后点云加入到目标点云坐标系 193 viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud_v2", v2); 194 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud_v2"); 195 std::cout << "All Point Cloud" << std::endl; 196 break; 197 case 3: //状态3：移除目标点云，只显示变换后点云 198 viewer.removePointCloud("target_cloud_v2",v2); 199 std::cout << "Transformed Point Cloud" << std::endl; 200 break; 201 case 0: //状态0：全部显示 202 viewer.addPointCloud (target_cloud, target_cloud_color_handler, "target_cloud_v2", v2); 203 viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target_cloud_v2"); 204 std::cout << "All Point Cloud" << std::endl; 205 break; 206 default: 207 break; 208 } 209 next_flag = false; //禁止进行下一次变换 210 } 211 } 212 return 0; 213 }
CMakeLists.txt
1 cmake_minimum_required(VERSION 2.6 FATAL_ERROR) 2 3 project(Fusion_Project) 4 5 find_package(PCL 1.6 REQUIRED) 6 7 include_directories(${PCL_INCLUDE_DIRS}) 8 link_directories(${PCL_LIBRARY_DIRS}) 9 add_definitions(${PCL_DEFINITIONS}) 10 11 add_executable (fusion fusion.cpp) 12 target_link_libraries (fusion ${PCL_LIBRARIES})
