第一周：读取XML深度数据并将其重建为三维点云

本周主要任务：学习PCL点云库，掌握利用PCL对点云处理的方法

任务时间：2014年9月1日-2014年9月7日

任务完成情况：完成了读取单幅xml深度数据，并重建三维点云并显示

任务涉及基本方法：

　　1.xml文件读取

　　　　http://www.cnblogs.com/xzd1575/p/3958172.html

　　2.OpenCV矩阵运算

　　　　http://www.cnblogs.com/xzd1575/p/3959120.html

　　　　http://www.cnblogs.com/xzd1575/p/3959113.html

　　3.PCL点云基本数据类型

　　　　http://www.cnblogs.com/xzd1575/p/3958192.html

　　　　http://www.cnblogs.com/xzd1575/p/3959181.html

　　4.CMake的基本使用方法

　　　　http://www.cnblogs.com/xzd1575/p/3959223.html　　　

程序如下：(包含xml2pcd.cpp 和 CMakeLists.txt)

1.xml2pcd.cpp

 //xml2pcd.cpp
 //函数：main()
 //功能：从.xml文件导入一张深度图像数据，利用数据重建三维点云，并保存到.pcd文件中
 //输入：导入文件名，输出文件名 例：可执行文件根目录下，命令行输入 xml2pcd depth1.xml.
 //创建时间：2014/09/03
 //最近更新时间：2014/09/07
 //创建者：肖泽东

 #include <iostream>
 #include <iomanip>
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/io.h>
 #include <pcl/point_types.h>
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/console/parse.h>
 #include <opencv2/opencv.hpp>
 #include <string>
 #include <stdexcept>

 static const int depthWidth = ;
 static const int depthHeight = ;

 void showHelp(char* program_name)
 {
     std::cout << std::endl;
     std::cout << "Usage: " << program_name << " depth_filename.xml " << std::endl;
     std::cout << "-h: Show this help." << std::endl;
 }

 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 文件
     std::vector<int> filenames;
     bool file_is_xml = false;

     filenames = pcl::console::parse_file_extension_argument(argc,argv,".xml");

     if (filenames.size() != )
     {
         showHelp(argv[]);
         return -;
     }
     else
     {
         file_is_xml = true;
     }

     //定义相关变量
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>& cloud = *cloud_ptr;    //点云
     cv::FileStorage fs;    //OpenCV 读XML文件流
     cv::Mat DepthData;    //深度数据矩阵
     std::string filename = argv[filenames[]];
         //    待读取.XML文件名

     //读取深度数据并显示深度图
     fs.open(filename,cv::FileStorage::READ);    //打开指定.xml文件
     if(!fs.isOpened())
     {
         std::cerr << "Error: cannot open .xml file";
         return -;
     }
     fs["Depth_Data"] >> DepthData;    //深度数据从文件导入至变量
     fs.release();    //释放xml文件

     //使用内参数矩阵进行摄像机坐标下的重建
     //设置内参数矩阵，并对其求逆
     float fc[] ={366.4484,356.6881};    //内参数 焦距参数
     //float fc[2] = {0};
     float cc[] ={265.3251,208.0765};    //内参数 图像中心参数
     //float cc[2] = {0};
     float K[][] = {fc[], , cc[], , fc[], cc[], , , };    //摄像机内参数矩阵K
     cv::Mat mK = cv::Mat(,,CV_32FC1,K);    //内参数K Mat类型变量
     cv::Mat mInvK(,,CV_32FC1);

     //将K数组值赋给Mat变量mK，并打印显示内参数矩阵
     //std::cout << "The intrinsic parameter matrix is :" << std::endl;
     //std::cout << mK << std::endl;

     //内参数矩阵mK求逆，并打印显示其逆矩阵
     try    //异常处理 针对K为奇异矩阵不可逆的情况
     {
         if (invert(mK,mInvK,cv::DECOMP_LU))    //矩阵求逆，如果矩阵为奇异矩阵，条件不成立
         {
             //std::cout << mInvK << std::endl;//打印显示矩阵数据
         }
         else    //K为奇异矩阵
         {
             throw std::invalid_argument("Error:Intrinsic parameter K is singular.");
                 //抛出异常“K为奇异矩阵”
         }
     }
     catch(std::invalid_argument& e)    //获取异常情况
     {
         std::cerr << e.what() << std::endl;    //打印异常提醒
         return -;
     }

     //初始化点云数据PCD文件头
     cloud.width    = depthHeight * depthWidth;
     cloud.height   = ;
     cloud.is_dense = false;
     cloud.points.resize (cloud.width * cloud.height);

     //使用深度数据，重建三维点云
     int row = , col = , pointId = ;
     for (row = ;row < depthHeight;row++)// row == y 遍历深度矩阵所有行
     {
         unsigned short* data = DepthData.ptr<unsigned short>(row);
         for(col = ;col < depthWidth;col++)// col == x 遍历深度矩阵所有列
         {
             if(*data> && *data<)    //取0.5m-1.5m范围深度数据
             {
                 pointId ++;
                 // [X,Y,Z]' = depth[x,y] * inv_K * [x,y,1]
                 cloud.points[pointId].x = *data * (col * mInvK.at<float>(,) +
                     row * mInvK.at<float>(,) + mInvK.at<float>(,));    //X

                 cloud.points[pointId].y = *data * (col * mInvK.at<float>(,) +
                     row * mInvK.at<float>(,) + mInvK.at<float>(,));    //Y

                 cloud.points[pointId].z = *data;    //Z

                 *data++ = (unsigned int)((*data - 500.0) * 255.0 / 1000.0) << ; //左移八位，data类型是16位，2^16 对应 256
             }
             else    // 其他范围数据无操作
             {
                 *data++ = ;    //只显示对应区域的深度图
             }
         }

     }
     //显示深度图
     cv::imshow(filename,DepthData);//显示截取深度数据
     cv::waitKey();    //等待50ms 用于等待显示完毕，防止显示图像不响应

     //重新给pcd文件头赋值
     cloud.width = pointId;    //unorgnized 数据，列宽即为点云个数，行数为1
     cloud.points.resize(cloud.width * cloud.height);    //点云数据个数

     //显示重建得到的点云数据
     pcl::visualization::CloudViewer viewer(filename);
     viewer.showCloud(cloud_ptr);

     //保存点云数据
     pcl::io::savePCDFileASCII (filename+".pcd", cloud);
     std::cerr << "Saved " << cloud.points.size () << " data points to test_pcd.pcd." << std::endl;

     //保持程序运行
     std::cout << std::endl << "Choose Depth Image Window and press ‘q’ to exit" << std::endl;
     while(!viewer.wasStopped())
     {
         if(cv::waitKey() == 'q')
             return();
     }
     return ();
 }

2.CMakeLists.txt

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)

project(Reseach_Project1)

find_package(PCL 1.3 REQUIRED)
find_package(OpenCV REQUIRED)

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

add_executable(xml2pcd xml2pcd.cpp)
target_link_libraries(xml2pcd ${PCL_LIBRARIES} ${OpenCV_LIBS})