Kinect 2 SDK + OpenCV 获取深度彩色图像

      感谢有钱的研究组，可以毫不眨眼买了3台Kinect2，其中一台还是备用的！真是有钱啊。

      导师拿到之后就发邮件说，哎，东西到啦咱们试试呗。妈蛋的他们都用Mac和Linux，但是Kinect2的SDK只能用在Win8上，据说之前只能用在Win 8.1上，最近才兼容到了Win8（2014年7月14日）. 于是这个任务就落在我身上了，因为只有我的个人电脑上用Win8，而且如果必须要用Win8.1他们还必须经过IT部门去买，但是我却可以直接弄到Win8.1，怎么弄到你懂的。于是导师对我嘿嘿一笑：“我就把Kinect放你这里了啊，你看着弄吧。”

       查了查也没有在网上找到相应的例子，看到一个外国红色社会主义友人写的博客介绍 http://www.kinectingforwindows.com/2014/03/03/gen-ii-kinect-basics-overview/  很系统很全面啊，但是我只想拿到depth和color图像，其它不关心啊，于是自己弄吧。

       首先，下载最新的Kinect 2 SDK  http://www.microsoft.com/en-us/kinectforwindows/develop/downloads-docs.aspx

       下载之后不要插入Kinect，最好也不用插入除了键盘鼠标以外的其它USB设备，然后安装SDK，安装完成之后插入Kinect，会有安装新设备的提示。安装完成之后可以去“开始”那里找到两个新安装的软件，一个是可以显示Kinect深度图，另外一个软件展示SDK中的各种例子程序。

        进入SDK的安装目录，可以找到sample这个文件夹，里面是四种语言编写的例子，其中native是C++的，managed是C#的，还有另外两种语言不熟悉，我就熟悉C++，反正只是试试的，就用C++了。

        这里只是获取深度和彩色图像，所以只用到 BasicDepthh 和 BasicColor 两个例子，因为要结合OpenCV显示保存图像，所以我就把例子中的代码抽离出来了。各位请看我生拉硬刨出来的代码：

#include "config.h"
#include "stdafx.h"
//#include "DepthBasics.h"

// 转换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;
}


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
	// 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];
	//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;

        hr = m_pKinectSensor->Open();

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

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

        SafeRelease(pDepthFrameSource);

		// 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_pDepthFrameReader)
    {
        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_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);
		}

		cv::imshow("depth", depthImg_show);
		cv::imshow("color", colorImg);
		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);

}

另外原始的Kinect深度图像是16bit的，转存为Mat可以用以下函数

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

	const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight);
	
	 while (pBuffer < pBufferEnd)
	{
		*p_mat = *pBuffer;
		p_mat++;
		++pBuffer;
	}
	return img;
}

在保存上，我记得OpenCV对16bit的视频没有保存的机制，所以只能把Kinect原始保存为png格式的图像，不能是视频。