使用opencv以及pcl将2D图像转换为3D点云

注：本文是在学习了高翔博士的相关博客（http://www.cnblogs.com/gaoxiang12/p/4652478.html）后写成的，纯粹作为一个学习笔记

1.在上一节即http://blog.csdn.net/zhuquan945/article/details/52788278中讲到的 代码根目录/src/ 文件夹中，即在/slam/src中新建一个名字为generatePointCloud.cpp的文件：

zhuquan@zhuquan-HP-Z230-Tower-Workstation:~$ cd ~/slam
zhuquan@zhuquan-HP-Z230-Tower-Workstation:~/slam$ cd src
zhuquan@zhuquan-HP-Z230-Tower-Workstation:~/slam/src$ touch generatePointCloud.cpp

并在generatePointCloud.cpp中写入如下内容：

// C++ 标准库
#include <iostream>
#include <string>
using namespace std;

// OpenCV 库
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

// PCL 库
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;

// 相机内参
const double camera_factor = 1000;
const double camera_cx = 325.5;
const double camera_cy = 253.5;
const double camera_fx = 518.0;
const double camera_fy = 519.0;

// 主函数
int main( int argc, char** argv )
{
// 读取./data/rgb.png和./data/depth.png，并转化为点云

// 图像矩阵
cv::Mat rgb, depth;
// 使用cv::imread()来读取图像
// API: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html?highlight=imread#cv2.imread
rgb = cv::imread( "./data/162025180951762.png" );
// rgb 图像是8UC3的彩色图像
// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
depth = cv::imread( "./data/162025372828732.png", -1 );

// 点云变量
// 使用智能指针，创建一个空点云。这种指针用完会自动释放。
PointCloud::Ptr cloud ( new PointCloud );
// 遍历深度图
for (int m = 0; m < depth.rows; m++)
for (int n=0; n < depth.cols; n++)
{
// 获取深度图中(m,n)处的值
ushort d = depth.ptr<ushort>(m)[n];
// d 可能没有值，若如此，跳过此点
if (d == 0)
continue;