本周主要任务：学习PCL点云库，掌握利用PCL对点云处理的方法
任务时间：2014年9月1日-2014年9月7日
任务完成情况：完成了读取单幅xml深度数据，并重建三维点云并显示
任务涉及基本方法：
1.xml文件读取
2.OpenCV矩阵运算
3.PCL点云基本数据类型
4.CMake的基本使用方法
程序如下：(包含xml2pcd.cpp 和 CMakeLists.txt)
1.xml2pcd.cpp
1 //xml2pcd.cpp
2 //函数：main()
3 //功能：从.xml文件导入一张深度图像数据，利用数据重建三维点云，并保存到.pcd文件中
4 //输入：导入文件名，输出文件名 例：可执行文件根目录下，命令行输入 xml2pcd depth1.xml.
5 //创建时间：2014/09/03
6 //最近更新时间：2014/09/07
7 //创建者：肖泽东
8
9 #include <iostream>
10 #include <iomanip>
11 #include <pcl/io/pcd_io.h>
12 #include <pcl/io/io.h>
13 #include <pcl/point_types.h>
14 #include <pcl/visualization/cloud_viewer.h>
15 #include <pcl/console/parse.h>
16 #include <opencv2/opencv.hpp>
17 #include <string>
18 #include <stdexcept>
19
20 static const int depthWidth = 512;
21 static const int depthHeight = 424;
22
23 void showHelp(char* program_name)
24 {
25 std::cout << std::endl;
26 std::cout << "Usage: " << program_name << " depth_filename.xml " << std::endl;
27 std::cout << "-h: Show this help." << std::endl;
28 }
29
30 int main (int argc, char** argv)
31 {
32 // 显示帮助文档
33 if(pcl::console::find_switch (argc,argv,"-h") || pcl::console::find_switch(argc,argv,"--help"))
34 {
35 showHelp(argv[0]);
36 return 0;
37 }
38
39 //从程序输入参量中获取深度文件名|xml 文件
40 std::vector<int> filenames;
41 bool file_is_xml = false;
42
43 filenames = pcl::console::parse_file_extension_argument(argc,argv,".xml");
44
45 if (filenames.size() != 1)
46 {
47 showHelp(argv[0]);
48 return -1;
49 }
50 else
51 {
52 file_is_xml = true;
53 }
54
55 //定义相关变量
56 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
57 pcl::PointCloud<pcl::PointXYZ>& cloud = *cloud_ptr; //点云
58 cv::FileStorage fs; //OpenCV 读XML文件流
59 cv::Mat DepthData; //深度数据矩阵
60 std::string filename = argv[filenames[0]];
61 // 待读取.XML文件名
62
63 //读取深度数据并显示深度图
64 fs.open(filename,cv::FileStorage::READ); //打开指定.xml文件
65 if(!fs.isOpened())
66 {
67 std::cerr << "Error: cannot open .xml file";
68 return -1;
69 }
70 fs["Depth_Data"] >> DepthData; //深度数据从文件导入至变量
71 fs.release(); //释放xml文件
72
73 //使用内参数矩阵进行摄像机坐标下的重建
74 //设置内参数矩阵，并对其求逆
75 float fc[2] ={366.4484,356.6881}; //内参数 焦距参数
76 //float fc[2] = {0};
77 float cc[2] ={265.3251,208.0765}; //内参数 图像中心参数
78 //float cc[2] = {0};
79 float K[3][3] = {fc[0], 0, cc[0], 0, fc[1], cc[1], 0, 0, 1}; //摄像机内参数矩阵K
80 cv::Mat mK = cv::Mat(3,3,CV_32FC1,K); //内参数K Mat类型变量
81 cv::Mat mInvK(3,3,CV_32FC1);
82
83 //将K数组值赋给Mat变量mK，并打印显示内参数矩阵
84 //std::cout << "The intrinsic parameter matrix is :" << std::endl;
85 //std::cout << mK << std::endl;
86
87 //内参数矩阵mK求逆，并打印显示其逆矩阵
88 try //异常处理 针对K为奇异矩阵不可逆的情况
89 {
90 if (invert(mK,mInvK,cv::DECOMP_LU)) //矩阵求逆，如果矩阵为奇异矩阵，条件不成立
91 {
92 //std::cout << mInvK << std::endl;//打印显示矩阵数据
93 }
94 else //K为奇异矩阵
95 {
96 throw std::invalid_argument("Error:Intrinsic parameter K is singular.");
97 //抛出异常“K为奇异矩阵”
98 }
99 }
100 catch(std::invalid_argument& e) //获取异常情况
101 {
102 std::cerr << e.what() << std::endl; //打印异常提醒
103 return -1;
104 }
105
106 //初始化点云数据PCD文件头
107 cloud.width = depthHeight * depthWidth;
108 cloud.height = 1;
109 cloud.is_dense = false;
110 cloud.points.resize (cloud.width * cloud.height);
111
112 //使用深度数据，重建三维点云
113 int row = 0, col = 0, pointId = 0;
114 for (row = 0;row < depthHeight;row++)// row == y 遍历深度矩阵所有行
115 {
116 unsigned short* data = DepthData.ptr<unsigned short>(row);
117 for(col = 0;col < depthWidth;col++)// col == x 遍历深度矩阵所有列
118 {
119 if(*data>500 && *data<1500) //取0.5m-1.5m范围深度数据
120 {
121 pointId ++;
122 // [X,Y,Z]' = depth[x,y] * inv_K * [x,y,1]
123 cloud.points[pointId].x = *data * (col * mInvK.at<float>(0,0) +
124 row * mInvK.at<float>(0,1) + mInvK.at<float>(0,2)); //X
125
126 cloud.points[pointId].y = *data * (col * mInvK.at<float>(1,0) +
127 row * mInvK.at<float>(1,1) + mInvK.at<float>(1,2)); //Y
128
129 cloud.points[pointId].z = *data; //Z
130
131 *data++ = (unsigned int)((*data - 500.0) * 255.0 / 1000.0) << 8; //左移八位，data类型是16位，2^16 对应 256
132 }
133 else // 其他范围数据无操作
134 {
135 *data++ = 0; //只显示对应区域的深度图
136 }
137 }
138
139 }
140 //显示深度图
141 cv::imshow(filename,DepthData);//显示截取深度数据
142 cv::waitKey(50); //等待50ms 用于等待显示完毕，防止显示图像不响应
143
144 //重新给pcd文件头赋值
145 cloud.width = pointId; //unorgnized 数据，列宽即为点云个数，行数为1
146 cloud.points.resize(cloud.width * cloud.height); //点云数据个数
147
148 //显示重建得到的点云数据
149 pcl::visualization::CloudViewer viewer(filename);
150 viewer.showCloud(cloud_ptr);
151
152 //保存点云数据
153 pcl::io::savePCDFileASCII (filename+".pcd", cloud);
154 std::cerr << "Saved " << cloud.points.size () << " data points to test_pcd.pcd." << std::endl;
155
156 //保持程序运行
157 std::cout << std::endl << "Choose Depth Image Window and press ‘q’ to exit" << std::endl;
158 while(!viewer.wasStopped())
159 {
160 if(cv::waitKey(1000) == 'q')
161 return(0);
162 }
163 return (0);
164 }
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})
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
