Last active
August 29, 2015 14:10
-
-
Save UnaNancyOwen/1d7b814830e408c868f7 to your computer and use it in GitHub Desktop.
Retrieve Body(Joint) from Kinect v2 using the CustomGrabber that inherit Kinect2Grabber
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
namespace pcl{ | |
class CustomGrabber : public pcl::Kinect2Grabber | |
{ | |
public: | |
CustomGrabber() | |
: source( nullptr ) | |
, reader( nullptr ) | |
{ | |
result = sensor->get_BodyFrameSource( &source ); | |
if( FAILED( result ) ){ | |
throw std::exception( "Exception : IKinectSensor::get_BodyFrameSource()" ); | |
} | |
result = source->OpenReader( &reader ); | |
if( FAILED( result ) ){ | |
throw std::exception( "Exception : IBodyFrameSource::OpenReader()" ); | |
} | |
thread = boost::thread( &CustomGrabber::threadFunction, this ); | |
}; | |
~CustomGrabber() | |
{ | |
SafeRelease( source ); | |
SafeRelease( reader ); | |
thread.join(); | |
}; | |
HRESULT GetJoints( unsigned int count, Joint* joints ){ | |
boost::mutex::scoped_lock( mutex ); | |
if( data[count].tracked ){ | |
for( int type = 0; type < JointType::JointType_Count; type++ ){ | |
joints[type] = data[count].joints[type]; | |
} | |
return S_OK; | |
} | |
else{ | |
return S_FALSE; | |
} | |
}; | |
private: | |
IBodyFrameSource* source; | |
IBodyFrameReader* reader; | |
boost::thread thread; | |
struct Data{ | |
Joint joints[JointType::JointType_Count]; | |
BOOLEAN tracked; | |
}; | |
Data data[BODY_COUNT]; | |
void threadFunction() | |
{ | |
while( !quit ){ | |
boost::unique_lock<boost::mutex> lock( mutex ); | |
IBodyFrame* frame = nullptr; | |
result = reader->AcquireLatestFrame( &frame ); | |
if( SUCCEEDED( result ) ){ | |
IBody* body[BODY_COUNT] = { 0 }; | |
result = frame->GetAndRefreshBodyData( BODY_COUNT, body ); | |
if( SUCCEEDED( result ) ){ | |
for( int count = 0; count < BODY_COUNT; count++ ){ | |
data[count].tracked = false; | |
result = body[count]->get_IsTracked( &data[count].tracked ); | |
if( SUCCEEDED( result ) && data[count].tracked ){ | |
body[count]->GetJoints( JointType::JointType_Count, data[count].joints ); | |
} | |
} | |
} | |
for( int count = 0; count < BODY_COUNT; count++ ){ | |
SafeRelease( body[count] ); | |
} | |
} | |
SafeRelease( frame ); | |
lock.unlock(); | |
} | |
}; | |
}; | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "stdafx.h" | |
// Disable Error C4996 that occur when using Boost.Signals2. | |
#ifdef _DEBUG | |
#define _SCL_SECURE_NO_WARNINGS | |
#endif | |
#include "kinect2_grabber.h" | |
#include <pcl/visualization/cloud_viewer.h> | |
namespace pcl{ | |
class CustomGrabber : public pcl::Kinect2Grabber | |
{ | |
public: | |
CustomGrabber() | |
: source( nullptr ) | |
, reader( nullptr ) | |
{ | |
result = sensor->get_BodyFrameSource( &source ); | |
if( FAILED( result ) ){ | |
throw std::exception( "Exception : IKinectSensor::get_BodyFrameSource()" ); | |
} | |
result = source->OpenReader( &reader ); | |
if( FAILED( result ) ){ | |
throw std::exception( "Exception : IBodyFrameSource::OpenReader()" ); | |
} | |
thread = boost::thread( &CustomGrabber::threadFunction, this ); | |
}; | |
~CustomGrabber() | |
{ | |
SafeRelease( source ); | |
SafeRelease( reader ); | |
thread.join(); | |
}; | |
HRESULT GetJoints( unsigned int count, Joint* joints ){ | |
boost::mutex::scoped_lock( mutex ); | |
if( data[count].tracked ){ | |
for( int type = 0; type < JointType::JointType_Count; type++ ){ | |
joints[type] = data[count].joints[type]; | |
} | |
return S_OK; | |
} | |
else{ | |
return S_FALSE; | |
} | |
}; | |
private: | |
IBodyFrameSource* source; | |
IBodyFrameReader* reader; | |
boost::thread thread; | |
struct Data{ | |
Joint joints[JointType::JointType_Count]; | |
BOOLEAN tracked; | |
}; | |
Data data[BODY_COUNT]; | |
void threadFunction() | |
{ | |
while( !quit ){ | |
boost::unique_lock<boost::mutex> lock( mutex ); | |
IBodyFrame* frame = nullptr; | |
result = reader->AcquireLatestFrame( &frame ); | |
if( SUCCEEDED( result ) ){ | |
IBody* body[BODY_COUNT] = { 0 }; | |
result = frame->GetAndRefreshBodyData( BODY_COUNT, body ); | |
if( SUCCEEDED( result ) ){ | |
for( int count = 0; count < BODY_COUNT; count++ ){ | |
data[count].tracked = false; | |
result = body[count]->get_IsTracked( &data[count].tracked ); | |
if( SUCCEEDED( result ) && data[count].tracked ){ | |
body[count]->GetJoints( JointType::JointType_Count, data[count].joints ); | |
} | |
} | |
} | |
for( int count = 0; count < BODY_COUNT; count++ ){ | |
SafeRelease( body[count] ); | |
} | |
} | |
SafeRelease( frame ); | |
lock.unlock(); | |
} | |
}; | |
}; | |
} | |
int _tmain( int argc, _TCHAR* argv[] ) | |
{ | |
// Create Cloud Viewer | |
pcl::visualization::CloudViewer viewer( "Point Cloud Viewer" ); | |
// Callback Function to be called when Updating Data | |
boost::function<void( const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& )> function = | |
[&viewer]( const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud ){ | |
if( !viewer.wasStopped() ){ | |
viewer.showCloud( cloud ); | |
} | |
}; | |
// Create CustomGrabber | |
pcl::CustomGrabber* grabber = new pcl::CustomGrabber(); | |
HRESULT result = S_OK; | |
// Regist Callback Function | |
grabber->registerCallback( function ); | |
// Start Retrieve Data | |
grabber->start(); | |
while( !viewer.wasStopped() ){ | |
// Get Other Data from CustomGrabber (e.g. Body) | |
for( int count = 0; count < BODY_COUNT; count++ ){ | |
Joint joint[JointType::JointType_Count]; | |
result = grabber->GetJoints( count, joint ); | |
if( SUCCEEDED( result ) ){ | |
/*for( int type = 0; type < JointType::JointType_Count; type++ ){ | |
CameraSpacePoint position = joint[type].Position; | |
float x = position.X; | |
float y = position.Y; | |
float z = position.Z; | |
}*/ | |
} | |
} | |
// Input Key ( Exit ESC key ) | |
if( GetKeyState( VK_ESCAPE ) < 0 ){ | |
break; | |
} | |
} | |
// Stop Retrieve Data | |
grabber->stop(); | |
return 0; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment