本周主要任务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})