Tracking-Coordinate/Depth.cpp at master · Tacode/Tracking-Coordinate · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
#include "Depth.h"
Depth::Depth()
{
}


Depth::~Depth()
{
	SafeRelease(pDepthFrameReader);
}


HRESULT Depth::InitKinect(IKinectSensor *kinect){

	pDepthFrameReader = NULL;
	pDepthFrameSource = NULL;

	hr = kinect->get_DepthFrameSource(&pDepthFrameSource);
	if (SUCCEEDED(hr)){
		hr = pDepthFrameSource->OpenReader(&pDepthFrameReader);
		if (FAILED(hr)){
			qDebug() << "Can not Open the Reader" << endl;
		}
	}
	SafeRelease(pDepthFrameSource);
	pixData = new UINT16[height*width];
	Capacity = height*width;
	return S_OK;
}

UINT16* Depth::updateDepth(Mat &depthImg){
	IDepthFrame* pDepthFrame = NULL;
	UINT nBufferSize_depth = 0;
	UINT16 *pBuffer_depth = NULL;
	hr = 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);
		}
		hr = pDepthFrame->AccessUnderlyingBuffer(&nBufferSize_depth, &pBuffer_depth);
		depthImg = ConvertMat(pBuffer_depth, width, height, nDepthMinReliableDistance, nDepthMaxReliableDistance);
		if (SUCCEEDED(hr)){
			//hr = pDepthFrame->CopyFrameDataToArray(Capacity, reinterpret_cast<UINT16 *> (tempImage.data));
			hr = pDepthFrame->CopyFrameDataToArray(Capacity, pixData);

		}
	}
	SafeRelease(pDepthFrame);
	return pixData;

}

Mat Depth::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;

}