vs2015+pcl1.8.1：从深度图像生成点云

#include <iostream>  
#include <string>  
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>  
#include <opencv2/highgui/highgui.hpp>  
#include <pcl/visualization/cloud_viewer.h>  
#include <iostream>  
#include <pcl/io/io.h>  
#include <pcl/io/pcd_io.h>  

using namespace std;
using namespace cv;

// 定义点云类型  
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 user_data;

void viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
{
	viewer.setBackgroundColor(0, 0, 0);

}

void viewerPsycho(pcl::visualization::PCLVisualizer& viewer)
{
	static unsigned count = 0;
	std::stringstream ss;
	ss << "Once per viewer loop: " << count++;
	viewer.removeShape("text", 0);
	viewer.addText(ss.str(), 200, 300, "text", 0);

	//FIXME: possible race condition here:  
	user_data++;
}


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

	Mat rgb, depth;
	// rgb 图像是8UC3的彩色图像  
	rgb = imread("colorseed.png");
	// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改  
	depth = imread("depthseed.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;
			// d 存在值，则向点云增加一个点  
			PointT p;

			// 计算这个点的空间坐标,因为点云上下颠倒，把y加一个负号  
			p.z = double(d) / camera_factor;
			p.x = (n - camera_cx) * p.z / camera_fx;
			p.y = -(m - camera_cy) * p.z / camera_fy;

			// 从rgb图像中获取它的颜色  
			// rgb是三通道的BGR格式图，所以按下面的顺序获取颜色  
			p.b = rgb.ptr<uchar>(m)[n * 3];
			p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
			p.r = rgb.ptr<uchar>(m)[n * 3 + 2];

			// 把p加入到点云中  
			cloud->points.push_back(p);
		}
	pcl::visualization::CloudViewer viewer("Cloud Viewer");
	viewer.showCloud(cloud);
	viewer.runOnVisualizationThreadOnce(viewerOneOff);
	viewer.runOnVisualizationThread(viewerPsycho);
	while (!viewer.wasStopped())
	{
		user_data++;
	}
	return 0;
}