第一周：读取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 = 512;
static const int depthHeight = 424;

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[0]);
        return 0;
    }

    //从程序输入参量中获取深度文件名|xml 文件
    std::vector<int> filenames;
    bool file_is_xml = false;

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

    if (filenames.size() != 1)
    {
        showHelp(argv[0]);
        return -1;
    }
    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[0]];
        //    待读取.XML文件名

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

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

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

    //使用深度数据，重建三维点云
    int row = 0, col = 0, pointId = 0;
    for (row = 0;row < depthHeight;row++)// row == y 遍历深度矩阵所有行
    {
        unsigned short* data = DepthData.ptr<unsigned short>(row);
        for(col = 0;col < depthWidth;col++)// col == x 遍历深度矩阵所有列
        {
            if(*data>500 && *data<1500)    //取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>(0,0) +
                    row * mInvK.at<float>(0,1) + mInvK.at<float>(0,2));    //X

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

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

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

    }    
    //显示深度图
    cv::imshow(filename,DepthData);//显示截取深度数据
    cv::waitKey(50);    //等待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(1000) == 'q')
            return(0);
    }
    return (0);
}

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