kinect 2.0生成点云，并用PCL存储

由kinect2.0的数据流来生成点云很容易实现，可是实用性不大，为了提高实用性，我写了一个通过读取kinect保存的jpg图片和其对应的深度txt文件，来生成点云的程序。生成的点云最终用pcl保存为ply或pcd格式的点云文件。我自己摸索的过程中走了很多弯路，分享出来帮助有相同需要的朋友们少走弯路。（注：碍于上传文件大小限制，我只上传了所有的代码以及一副实例图片和对应的深度txt文件，需自己正确配置opencv+pcl+kinect2.0）

//此程序功能：从jpg和txt得到点云！

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace cv;
using namespace std;
FILE* fd;
IKinectSensor* sensor;             // Kinect sensor
void retxt（UINT16 *data FILE* fp char* name）
{
	fp = fopen（name “r“）;

	for （int i = 0; i < 512 * 424; i++）
	{
		fscanf（fp “%d“ data）;
		data++;
	}
	fclose（fp）;
}

Mat image = imread（“p1.jpg“）;//在此处读入jpg图片，要求kinect2.0尺寸的（1920*1080）

void Convertrgbdata（unsigned char pBuffer[1920 * 1080 * 4] Mat img）
{

	uchar* p_mat = img.data;

	const unsigned char* pBufferEnd = pBuffer + （1920 * 1080 * 4）;

	while （pBuffer < pBufferEnd）
	{
		*pBuffer = *p_mat;
		p_mat++; pBuffer++;
		*pBuffer = *p_mat;
		p_mat++; pBuffer++;
		*pBuffer = *p_mat;
		p_mat++; pBuffer++;
		++pBuffer;
	}
}

const int width = 512;
const int height = 424;
const int colorwidth = 1920;
const int colorheight = 1080;

// Intermediate Buffers
unsigned char rgbimage[colorwidth*colorheight * 4];    // Stores RGB color image
ColorSpacePoint depth2rgb[width*height];             // Maps depth pixels to rgb pixels
CameraSpacePoint depth2xyz[width*height];			 // Maps depth pixels to 3d coordinates

// Kinect Variables
ICoordinateMapper* mapper;         // Converts between depth color and 3d coordinates

float* ptr = NULL;
float* ptrc = NULL;

bool initKinect（） {
	if （FAILED（GetDefaultKinectSensor（&sensor））） {
		return false;
	}
	if （sensor）{
		sensor->get_CoordinateMapper（&mapper）;

		sensor->Open（）;

	}
	else {
		return false;
	}
}

void getDepthData（UINT16* buf float* dest char*name1） {
	retxt（buf fd name1）;
	// Write vertex coordinates
	mapper->MapDepthframeToCameraSpace（width*height buf width*height depth2xyz）;
	//	float* fdest = （float*）dest;
	for （int i = 0; i < 512 * 424; i++） {
		*dest++ = depth2xyz[i].X;
		*dest++ = depth2xyz[i].Y;
		*dest++ = depth2xyz[i].Z;
	}

	// Fill in depth2rgb map
	mapper->MapDepthframeToColorSpace（width*height buf width*height depth2rgb）;
}

void getRgbData（unsigned char* rgbdata float* dest） {

	Convertrgbdata（rgbdata image）;
	// Write color array for vertices

	for （int i = 0; i < width*height; i++） {
		ColorSpacePoint p = depth2rgb[i];
		// Check if color pixel coordinates are in bounds
		if （p.X < 0 || p.Y < 0 || p.X > colorwidth || p.Y > colorheight） {
			*dest++ = -1;
			*dest++ = -1;
			*dest++ = -1;
		}
		else {
			int idx = （int）p.X + colorwidth*（int）p.Y;
			*dest++ = rgbdata[4 * idx + 2];
			*dest++ = rgbdata[4 * idx + 1];
			*dest++ = rgbdata[4 * idx + 0];
		}
		// Don‘t copy alpha channel
	}

}



int main（）
{
	initKinect（）;
	UINT16 *depthArray = new UINT16[512 * 424];//深度数据是16位unsigned int

	ptr = （float *）malloc（512 * 424 * 3 * sizeof（float））;
	ptrc = （float *）malloc（512 * 424 * 3 * sizeof（float））;
	getDepthData（depthArray ptr “p1.txt“）;//在此处读入对应于图片的深度数

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       3989  2016-09-18 19:45  pcl+kinect\kinect+pcl.cpp

     文件     416594  2016-09-13 09:28  pcl+kinect\p1.jpg

     文件    1226056  2016-09-13 09:28  pcl+kinect\p1.txt

     目录          0  2016-09-18 19:46  pcl+kinect

----------- ---------  ---------- -----  ----

              1646639                    4