//此程序功能：從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?	{
		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?	{
		*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?		*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?		ColorSpacePoint?p?=?depth2rgb[i];
		//?Check?if?color?pixel?coordinates?are?in?bounds
		if?（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