void KinectDevice::getFrameInformation() {
// Get the frame information
IDepthFrameSource *depthSrc;
IColorFrameSource *colorSrc;
IInfraredFrameSource *irSrc;
ILongExposureInfraredFrameSource *hdirSrc;
IBodyIndexFrameSource *indexSrc;
IFrameDescription *depthDesc, *colorDesc, *irDesc, *hdirDesc, *indexDesc;
if (_streams & Streams::DEPTH_STREAM) {
_sensor->get_DepthFrameSource(&depthSrc);
depthSrc->get_FrameDescription(&depthDesc);
depthFrameInfo = FrameInfo(depthDesc);
// Min/max vals
depthSrc->get_DepthMinReliableDistance(&depthFrameInfo.minVal);
depthSrc->get_DepthMaxReliableDistance(&depthFrameInfo.maxVal);
// Allocate
depthData = std::shared_ptr<uint16_t>(new uint16_t[depthFrameInfo.frameSize]);
prevDepthData = std::shared_ptr<uint16_t>(new uint16_t[depthFrameInfo.frameSize]);
}
else {
depthData = nullptr;
prevDepthData = nullptr;
}
if (_streams & Streams::COLOR_STREAM) {
_sensor->get_ColorFrameSource(&colorSrc);
colorSrc->get_FrameDescription(&colorDesc);
colorFrameInfo = FrameInfo(colorDesc);
colorData = std::shared_ptr<uint16_t>(new uint16_t[colorFrameInfo.frameSize]);
}
if (_streams & Streams::IR_STREAM) {
_sensor->get_InfraredFrameSource(&irSrc);
irSrc->get_FrameDescription(&irDesc);
irFrameInfo = FrameInfo(irDesc);
irData = std::shared_ptr<uint16_t>(new uint16_t[irFrameInfo.frameSize]);
}
if (_streams & Streams::HDIR_STREAM) {
_sensor->get_LongExposureInfraredFrameSource(&hdirSrc);
hdirSrc->get_FrameDescription(&hdirDesc);
hdirFrameInfo = FrameInfo(hdirDesc);
hdirData = std::shared_ptr<uint16_t>(new uint16_t[hdirFrameInfo.frameSize]);
}
if (_streams & Streams::INDEX_STREAM) {
_sensor->get_BodyIndexFrameSource(&indexSrc);
indexSrc->get_FrameDescription(&indexDesc);
indexFrameInfo = FrameInfo(indexDesc);
indexData = std::shared_ptr<BYTE>(new BYTE[indexFrameInfo.frameSize]);
}
}
bool BodyIndexStream::open()
{
if (!m_Device->isOpen()) {
ofLogWarning("ofxKinect2::BodyIndexStream") << "No ready Kinect2 found.";
return false;
}
m_IsInvert = true;
IBodyIndexFrameSource *frameSource = nullptr;
HRESULT hr = E_FAIL;
hr = m_Device->get().kinect2->get_BodyIndexFrameSource(&frameSource);
if (SUCCEEDED(hr)) {
hr = frameSource->OpenReader(&m_StreamHandle.bodyIndexFrameReader);
if (SUCCEEDED(hr)) {
IFrameDescription *frameDescription = nullptr;
frameSource->get_FrameDescription(&frameDescription);
if (SUCCEEDED(hr)) {
int resX, resY = 0;
hr = frameDescription->get_Width(&resX);
hr = frameDescription->get_Width(&resY);
m_Frame.mode.resolutionX = resX;
m_Frame.mode.resolutionY = resY;
m_Frame.width = resX;
m_Frame.height = resY;
m_DoubleBuffer.allocate(resX, resY, 4);
}
safeRelease(frameDescription);
}
}
safeRelease(frameSource);
if (FAILED(hr)) {
ofLogWarning("ofxKinect2::BodyIndexStream") << "Can't open stream.";
return false;
}
return Stream::open();
}
int main(int argc, char** argv)
{
// 1a. Get default Sensor
cout << "Try to get default sensor" << endl;
IKinectSensor* pSensor = nullptr;
if (GetDefaultKinectSensor(&pSensor) != S_OK)
{
cerr << "Get Sensor failed" << endl;
return -1;
}
// 1b. Open sensor
cout << "Try to open sensor" << endl;
if (pSensor->Open() != S_OK)
{
cerr << "Can't open sensor" << endl;
return -1;
}
// 2a. Get frame source
cout << "Try to get body index source" << endl;
IBodyIndexFrameSource* pFrameSource = nullptr;
if (pSensor->get_BodyIndexFrameSource(&pFrameSource) != S_OK)
{
cerr << "Can't get body index frame source" << endl;
return -1;
}
// 2b. Get frame description
cout << "get body index frame description" << endl;
int iWidth = 0;
int iHeight = 0;
IFrameDescription* pFrameDescription = nullptr;
if (pFrameSource->get_FrameDescription(&pFrameDescription) == S_OK)
{
pFrameDescription->get_Width(&iWidth);
pFrameDescription->get_Height(&iHeight);
}
pFrameDescription->Release();
pFrameDescription = nullptr;
// 3a. get frame reader
cout << "Try to get body index frame reader" << endl;
IBodyIndexFrameReader* pFrameReader = nullptr;
if (pFrameSource->OpenReader(&pFrameReader) != S_OK)
{
cerr << "Can't get body index frame reader" << endl;
return -1;
}
// 2c. release Frame source
cout << "Release frame source" << endl;
pFrameSource->Release();
pFrameSource = nullptr;
// Prepare OpenCV data
cv::Mat mImg(iHeight, iWidth, CV_8UC3);
cv::namedWindow("Body Index Image");
// color array
cv::Vec3b aColorTable[7] = {
cv::Vec3b(255,0,0),
cv::Vec3b(0,255,0),
cv::Vec3b(0,0,255),
cv::Vec3b(255,255,0),
cv::Vec3b(255,0,255),
cv::Vec3b(0,255,255),
cv::Vec3b(0,0,0),
};
// Enter main loop
while (true)
{
// 4a. Get last frame
IBodyIndexFrame* pFrame = nullptr;
if (pFrameReader->AcquireLatestFrame(&pFrame) == S_OK)
{
// 4c. Fill OpenCV image
UINT uSize = 0;
BYTE* pBuffer = nullptr;
if (pFrame->AccessUnderlyingBuffer(&uSize,&pBuffer) == S_OK)
{
for (int y = 0; y < iHeight; ++y)
{
for (int x = 0; x < iWidth; ++x)
{
int uBodyIdx = pBuffer[x + y * iWidth];
if (uBodyIdx < 6)
mImg.at<cv::Vec3b>(y, x) = aColorTable[uBodyIdx];
else
mImg.at<cv::Vec3b>(y, x) = aColorTable[6];
}
}
cv::imshow("Body Index Image", mImg);
}
else
{
cerr << "Data access error" << endl;
}
// 4e. release frame
pFrame->Release();
}
// 4f. check keyboard input
if (cv::waitKey(30) == VK_ESCAPE){
break;
}
}
// 3b. release frame reader
cout << "Release frame reader" << endl;
pFrameReader->Release();
pFrameReader = nullptr;
// 1c. Close Sensor
cout << "close sensor" << endl;
pSensor->Close();
// 1d. Release Sensor
cout << "Release sensor" << endl;
pSensor->Release();
pSensor = nullptr;
return 0;
}
int _tmain( int argc, _TCHAR* argv[] )
{
cv::setUseOptimized( true );
// Sensor
IKinectSensor* pSensor;
HRESULT hResult = S_OK;
hResult = GetDefaultKinectSensor( &pSensor );
if( FAILED( hResult ) ){
std::cerr << "Error : GetDefaultKinectSensor" << std::endl;
return -1;
}
hResult = pSensor->Open();
if( FAILED( hResult ) ){
std::cerr << "Error : IKinectSensor::Open()" << std::endl;
return -1;
}
// Source
IBodyIndexFrameSource* pBodyIndexSource;
hResult = pSensor->get_BodyIndexFrameSource( &pBodyIndexSource );
if( FAILED( hResult ) ){
std::cerr << "Error : IKinectSensor::get_BodyIndexFrameSource()" << std::endl;
return -1;
}
// Reader
IBodyIndexFrameReader* pBodyIndexReader;
hResult = pBodyIndexSource->OpenReader( &pBodyIndexReader );
if( FAILED( hResult ) ){
std::cerr << "Error : IBodyIndexFrameSource::OpenReader()" << std::endl;
return -1;
}
// Description
IFrameDescription* pDescription;
hResult = pBodyIndexSource->get_FrameDescription( &pDescription );
if( FAILED( hResult ) ){
std::cerr << "Error : IBodyIndexFrameSource::get_FrameDescription()" << std::endl;
return -1;
}
int width = 0;
int height = 0;
pDescription->get_Width( &width ); // 512
pDescription->get_Height( &height ); // 424
cv::Mat bodyIndexMat( height, width, CV_8UC3 );
cv::namedWindow( "BodyIndex" );
// Color Table
cv::Vec3b color[BODY_COUNT];
color[0] = cv::Vec3b( 255, 0, 0 );
color[1] = cv::Vec3b( 0, 255, 0 );
color[2] = cv::Vec3b( 0, 0, 255 );
color[3] = cv::Vec3b( 255, 255, 0 );
color[4] = cv::Vec3b( 255, 0, 255 );
color[5] = cv::Vec3b( 0, 255, 255 );
while( 1 ){
// Frame
IBodyIndexFrame* pBodyIndexFrame = nullptr;
hResult = pBodyIndexReader->AcquireLatestFrame( &pBodyIndexFrame );
if( SUCCEEDED( hResult ) ){
unsigned int bufferSize = 0;
unsigned char* buffer = nullptr;
hResult = pBodyIndexFrame->AccessUnderlyingBuffer( &bufferSize, &buffer );
if( SUCCEEDED( hResult ) ){
for( int y = 0; y < height; y++ ){
for( int x = 0; x < width; x++ ){
unsigned int index = y * width + x;
if( buffer[index] != 0xff ){
bodyIndexMat.at<cv::Vec3b>( y, x ) = color[buffer[index]];
}
else{
bodyIndexMat.at<cv::Vec3b>( y, x ) = cv::Vec3b( 0, 0, 0 );
}
}
}
}
}
SafeRelease( pBodyIndexFrame );
cv::imshow( "BodyIndex", bodyIndexMat );
if( cv::waitKey( 30 ) == VK_ESCAPE ){
break;
}
}
SafeRelease( pBodyIndexSource );
SafeRelease( pBodyIndexReader );
SafeRelease( pDescription );
if( pSensor ){
pSensor->Close();
}
SafeRelease( pSensor );
cv::destroyAllWindows();
return 0;
}
