Kinect2.0 winSDK彩图深度图红外图Opencv显示

代码是通过提取sample里的代码编写的，同时读取深度红外和彩色图，整个流程还是很清晰的，sensor -source-reader - frame - data。详细流程参考http://blog.csdn.net/dustpg/article/details/37987239，这篇博文非常清楚。

//#include "config.h"
#include "stdafx.h"
#include "DepthBasics.h"
#include <opencv2/opencv.hpp>

using namespace std;

#define InfraredSourceValueMaximum static_cast<float>(USHRT_MAX)
#define InfraredOutputValueMinimum 0.01f 
#define InfraredOutputValueMaximum 1.0f
#define InfraredSceneValueAverage 0.08f
#define InfraredSceneStandardDeviations 3.0f

// 转换depth图像到cv::Mat
cv::Mat ConvertMat(const UINT16* pBuffer, int nWidth, int nHeight, USHORT nMinDepth, USHORT nMaxDepth)
{
	cv::Mat img(nHeight, nWidth, CV_8UC3);
	uchar* p_mat = img.data;

	const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight);

	while (pBuffer < pBufferEnd)
	{
		USHORT depth = *pBuffer;

		BYTE intensity = static_cast<BYTE>((depth >= nMinDepth) && (depth <= nMaxDepth) ? (depth % 256) : 0);

		*p_mat = intensity;
		p_mat++;
		*p_mat = intensity;
		p_mat++;
		*p_mat = intensity;
		p_mat++;

		++pBuffer;
	}
	return img;
}
// 转换color图像到cv::Mat
cv::Mat ConvertMat(const RGBQUAD* pBuffer, int nWidth, int nHeight)
{
	cv::Mat img(nHeight, nWidth, CV_8UC3);
	uchar* p_mat = img.data;

	const RGBQUAD* pBufferEnd = pBuffer + (nWidth * nHeight);

	while (pBuffer < pBufferEnd)
	{
		*p_mat = pBuffer->rgbBlue;
		p_mat++;
		*p_mat = pBuffer->rgbGreen;
		p_mat++;
		*p_mat = pBuffer->rgbRed;
		p_mat++;

		++pBuffer;
	}
	return img;
}

cv::Mat ConvertMat(const UINT16* pBuffer, int nWidth, int nHeight)
{
	cv::Mat img(nHeight, nWidth, CV_8UC3);
	uchar* p_mat = img.data;

	const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight);

	while (pBuffer < pBufferEnd)
	{
		USHORT depth = *pBuffer;

		float intensityRatio = static_cast<float>(*pBuffer) / InfraredSourceValueMaximum;

		intensityRatio /= InfraredSceneValueAverage * InfraredSceneStandardDeviations;

		intensityRatio = min(InfraredOutputValueMaximum, intensityRatio);

		intensityRatio = max(InfraredOutputValueMinimum, intensityRatio);

		byte intensity = static_cast<byte>(intensityRatio * 255.0f);

		*p_mat = intensity;
		p_mat++;
		*p_mat = intensity;
		p_mat++;
		*p_mat = intensity;
		p_mat++;

		++pBuffer;
	}
	return img;
}

void main()
{
	
	int depth_width = 512; //depth图像就是这么小
	int depth_height = 424;
	int color_widht = 1920; //color图像就是辣么大
	int color_height = 1080;

	cv::Mat depthImg_show = cv::Mat::zeros(depth_height, depth_width, CV_8UC3);//原始UINT16 深度图像不适合用来显示，所以需要砍成8位的就可以了，但是显示出来也不是非常好，最好能用原始16位图像颜色编码，凑合着看了
	cv::Mat depthImg = cv::Mat::zeros(depth_height, depth_width, CV_16UC1);//the depth image
	cv::Mat colorImg = cv::Mat::zeros(color_height, color_widht, CV_8UC3);//the color image
	cv::Mat infraImg = cv::Mat::zeros(depth_height, depth_height, CV_8UC3);//the infrared image
	// Current Kinect
	IKinectSensor* m_pKinectSensor = NULL;
	// Depth reader
	IDepthFrameReader*  m_pDepthFrameReader = NULL;
	// Color reader
	IColorFrameReader*  m_pColorFrameReader = NULL;
	RGBQUAD* m_pColorRGBX = new RGBQUAD[color_widht * color_height];
	// Infrared reader
	IInfraredFrameReader* m_pInfraredReader = NULL;
	//open it!
	HRESULT hr;

	hr = GetDefaultKinectSensor(&m_pKinectSensor);
	if (FAILED(hr))
	{
		cout << "FUCK! Can not find the Kinect!" << endl;
		cv::waitKey(0);
		exit(0);
	}

	if (m_pKinectSensor)
	{
		// Initialize the Kinect and get the depth reader
		IDepthFrameSource* pDepthFrameSource = NULL;
		IInfraredFrameSource* pInfraredFrameSource = nullptr;

		hr = m_pKinectSensor->Open();

		if (SUCCEEDED(hr))
		{
			hr = m_pKinectSensor->get_DepthFrameSource(&pDepthFrameSource);
			hr = m_pKinectSensor->get_InfraredFrameSource(&pInfraredFrameSource);
		}

		if (SUCCEEDED(hr))
		{
			hr = pDepthFrameSource->OpenReader(&m_pDepthFrameReader);
			hr = pInfraredFrameSource->OpenReader(&m_pInfraredReader);
		}

		SafeRelease(pDepthFrameSource);
		SafeRelease(pInfraredFrameSource);

		// for color
		// Initialize the Kinect and get the color reader
		IColorFrameSource* pColorFrameSource = NULL;
		if (SUCCEEDED(hr))
		{
			hr = m_pKinectSensor->get_ColorFrameSource(&pColorFrameSource);
		}

		if (SUCCEEDED(hr))
		{
			hr = pColorFrameSource->OpenReader(&m_pColorFrameReader);
		}

		SafeRelease(pColorFrameSource);
	}

	//valify the depth reader
	if (!m_pDepthFrameReader)
	{
		cout << "FUCK! Can not find the m_pDepthFrameReader!" << endl;
		cv::waitKey(0);
		exit(0);
	}
	//valify the color reader
	if (!m_pColorFrameReader)
	{
		cout << "FUCK! Can not find the m_pColorFrameReader!" << endl;
		cv::waitKey(0);
		exit(0);
	}
	// get the data!
	UINT nBufferSize_depth = 0;
	UINT16 *pBuffer_depth = NULL;
	UINT nBufferSize_infra = 0;
	UINT16 *pBuffer_infra = NULL;
	UINT nBufferSize_coloar = 0;
	RGBQUAD *pBuffer_color = NULL;

	char key = 0;

	while (true) // 貌似要一直尝试，不一定每次都能读取到图像
	{
		IDepthFrame* pDepthFrame = NULL;
		HRESULT hr = m_pDepthFrameReader->AcquireLatestFrame(&pDepthFrame);
		if (SUCCEEDED(hr))
		{
			USHORT nDepthMinReliableDistance = 0;
			USHORT nDepthMaxReliableDistance = 0;
			if (SUCCEEDED(hr))
			{
				hr = pDepthFrame->get_DepthMinReliableDistance(&nDepthMinReliableDistance);
			}

			if (SUCCEEDED(hr))
			{
				hr = pDepthFrame->get_DepthMaxReliableDistance(&nDepthMaxReliableDistance);
			}
			if (SUCCEEDED(hr))
			{
				hr = pDepthFrame->AccessUnderlyingBuffer(&nBufferSize_depth, &pBuffer_depth);
				depthImg_show = ConvertMat(pBuffer_depth, depth_width, depth_height, nDepthMinReliableDistance, nDepthMaxReliableDistance);
			}
		}
		SafeRelease(pDepthFrame);


		//for color
		IColorFrame* pColorFrame = NULL;
		hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame);
		ColorImageFormat imageFormat = ColorImageFormat_None;
		if (SUCCEEDED(hr))
		{
			ColorImageFormat imageFormat = ColorImageFormat_None;
			if (SUCCEEDED(hr))
			{
				hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
			}
			if (SUCCEEDED(hr))
			{
				hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
			}
			if (SUCCEEDED(hr))
			{
				if (imageFormat == ColorImageFormat_Bgra)//这里有两个format，不知道具体含义，大概一个预先分配内存，一个需要自己开空间吧
				{
					hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize_coloar, reinterpret_cast<BYTE**>(&pBuffer_color));
				}
				else if (m_pColorRGBX)
				{
					pBuffer_color = m_pColorRGBX;
					nBufferSize_coloar = color_widht * color_height * sizeof(RGBQUAD);
					hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize_coloar, reinterpret_cast<BYTE*>(pBuffer_color), ColorImageFormat_Bgra);
				}
				else
				{
					hr = E_FAIL;
				}
				colorImg = ConvertMat(pBuffer_color, color_widht, color_height);
			}

			SafeRelease(pColorFrame);
		}

		IInfraredFrame* pInfraredFrame = NULL;

		hr = m_pInfraredReader->AcquireLatestFrame(&pInfraredFrame);

		if (SUCCEEDED(hr))
		{
			IFrameDescription* pFrameDescription = NULL;
			INT64 nTime = 0;
			int nWidth = 0;
			int nHeight = 0;
			UINT nBufferSize = 0;
			UINT16 *pBuffer = NULL;

			hr = pInfraredFrame->get_RelativeTime(&nTime);

			if (SUCCEEDED(hr))
			{
				hr = pInfraredFrame->get_FrameDescription(&pFrameDescription);
			}

			if (SUCCEEDED(hr))
			{
				hr = pFrameDescription->get_Width(&nWidth);
			}

			if (SUCCEEDED(hr))
			{
				hr = pFrameDescription->get_Height(&nHeight);
			}

			if (SUCCEEDED(hr))
			{
				hr = pInfraredFrame->AccessUnderlyingBuffer(&nBufferSize, &pBuffer);
			}
			infraImg = ConvertMat(pBuffer, depth_width, depth_height);
			SafeRelease(pFrameDescription);
		}

		SafeRelease(pInfraredFrame);

		cv::imshow("depth", depthImg_show);
		cv::imshow("color", colorImg);
		cv::imshow("infrared",infraImg);
		key = cv::waitKey(1);
		if (key == 27)
		{
			break;
		}
	}


	if (m_pColorRGBX)
	{
		delete[] m_pColorRGBX;
		m_pColorRGBX = NULL;
	}
	// close the Kinect Sensor
	if (m_pKinectSensor)
	{
		m_pKinectSensor->Close();
	}
	SafeRelease(m_pKinectSensor);

}