371 lines
10 KiB
C++
371 lines
10 KiB
C++
|
#include "app.h"
|
||
|
|
#include "util.h"
|
||
|
|
|
||
|
|
#include <thread>
|
||
|
|
#include <chrono>
|
||
|
|
|
||
|
|
#include <ppl.h>
|
||
|
|
|
||
|
|
// Joint Smoothing
|
||
|
|
#define SMOOTH
|
||
|
|
|
||
|
|
// Constructor
|
||
|
|
Kinect::Kinect()
|
||
|
|
{
|
||
|
|
// Initialize
|
||
|
|
initialize();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Destructor
|
||
|
|
Kinect::~Kinect()
|
||
|
|
{
|
||
|
|
// Finalize
|
||
|
|
finalize();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Processing
|
||
|
|
void Kinect::run()
|
||
|
|
{
|
||
|
|
// Main Loop
|
||
|
|
while( true ){
|
||
|
|
// Update Data
|
||
|
|
update();
|
||
|
|
|
||
|
|
// Draw Data
|
||
|
|
draw();
|
||
|
|
|
||
|
|
// Show Data
|
||
|
|
show();
|
||
|
|
|
||
|
|
// Key Check
|
||
|
|
const int key = cv::waitKey( 10 );
|
||
|
|
if( key == VK_ESCAPE ){
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Initialize
|
||
|
|
void Kinect::initialize()
|
||
|
|
{
|
||
|
|
cv::setUseOptimized( true );
|
||
|
|
|
||
|
|
// Initialize Sensor
|
||
|
|
initializeSensor();
|
||
|
|
|
||
|
|
// Initialize Color
|
||
|
|
initializeColor();
|
||
|
|
|
||
|
|
// Initialize Body
|
||
|
|
initializeBody();
|
||
|
|
|
||
|
|
// Wait a Few Seconds until begins to Retrieve Data from Sensor ( about 2000-[ms] )
|
||
|
|
std::this_thread::sleep_for( std::chrono::seconds( 2 ) );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Initialize Sensor
|
||
|
|
inline void Kinect::initializeSensor()
|
||
|
|
{
|
||
|
|
// Open Sensor
|
||
|
|
ERROR_CHECK( GetDefaultKinectSensor( &kinect ) );
|
||
|
|
|
||
|
|
ERROR_CHECK( kinect->Open() );
|
||
|
|
|
||
|
|
// Check Open
|
||
|
|
BOOLEAN isOpen = FALSE;
|
||
|
|
ERROR_CHECK( kinect->get_IsOpen( &isOpen ) );
|
||
|
|
if( !isOpen ){
|
||
|
|
throw std::runtime_error( "failed IKinectSensor::get_IsOpen( &isOpen )" );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Retrieve Coordinate Mapper
|
||
|
|
ERROR_CHECK( kinect->get_CoordinateMapper( &coordinateMapper ) );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Initialize Color
|
||
|
|
inline void Kinect::initializeColor()
|
||
|
|
{
|
||
|
|
// Open Color Reader
|
||
|
|
ComPtr<IColorFrameSource> colorFrameSource;
|
||
|
|
ERROR_CHECK( kinect->get_ColorFrameSource( &colorFrameSource ) );
|
||
|
|
ERROR_CHECK( colorFrameSource->OpenReader( &colorFrameReader ) );
|
||
|
|
|
||
|
|
// Retrieve Color Description
|
||
|
|
ComPtr<IFrameDescription> colorFrameDescription;
|
||
|
|
ERROR_CHECK( colorFrameSource->CreateFrameDescription( ColorImageFormat::ColorImageFormat_Bgra, &colorFrameDescription ) );
|
||
|
|
ERROR_CHECK( colorFrameDescription->get_Width( &colorWidth ) ); // 1920
|
||
|
|
ERROR_CHECK( colorFrameDescription->get_Height( &colorHeight ) ); // 1080
|
||
|
|
ERROR_CHECK( colorFrameDescription->get_BytesPerPixel( &colorBytesPerPixel ) ); // 4
|
||
|
|
|
||
|
|
// Allocation Color Buffer
|
||
|
|
colorBuffer.resize( colorWidth * colorHeight * colorBytesPerPixel );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Initialize Body
|
||
|
|
inline void Kinect::initializeBody()
|
||
|
|
{
|
||
|
|
// Open Body Reader
|
||
|
|
ComPtr<IBodyFrameSource> bodyFrameSource;
|
||
|
|
ERROR_CHECK( kinect->get_BodyFrameSource( &bodyFrameSource ) );
|
||
|
|
ERROR_CHECK( bodyFrameSource->OpenReader( &bodyFrameReader ) );
|
||
|
|
|
||
|
|
// Initialize Body Buffer
|
||
|
|
Concurrency::parallel_for_each( bodies.begin(), bodies.end(), []( IBody*& body ){
|
||
|
|
SafeRelease( body );
|
||
|
|
} );
|
||
|
|
|
||
|
|
#ifdef SMOOTH
|
||
|
|
// Set Smoothing Fileter Parameters
|
||
|
|
Sample::TRANSFORM_SMOOTH_PARAMETERS smoothingParams;
|
||
|
|
smoothingParams.fSmoothing = 0.25f; // [0..1], lower values closer to raw data
|
||
|
|
smoothingParams.fCorrection = 0.25f; // [0..1], lower values slower to correct towards the raw data
|
||
|
|
smoothingParams.fPrediction = 0.25f; // [0..n], the number of frames to predict into the future
|
||
|
|
smoothingParams.fJitterRadius = 0.03f; // The radius in meters for jitter reduction
|
||
|
|
smoothingParams.fMaxDeviationRadius = 0.05f; // The maximum radius in meters that filtered positions are allowed to deviate from raw data
|
||
|
|
|
||
|
|
// Create Holt Double Exponential Smoothing Filter
|
||
|
|
for( Sample::FilterDoubleExponential filter : filters ){
|
||
|
|
filter.Init( smoothingParams.fSmoothing, smoothingParams.fCorrection, smoothingParams.fPrediction, smoothingParams.fJitterRadius, smoothingParams.fMaxDeviationRadius );
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
// Color Table for Visualization
|
||
|
|
colors[0] = cv::Vec3b( 255, 0, 0 ); // Blue
|
||
|
|
colors[1] = cv::Vec3b( 0, 255, 0 ); // Green
|
||
|
|
colors[2] = cv::Vec3b( 0, 0, 255 ); // Red
|
||
|
|
colors[3] = cv::Vec3b( 255, 255, 0 ); // Cyan
|
||
|
|
colors[4] = cv::Vec3b( 255, 0, 255 ); // Magenta
|
||
|
|
colors[5] = cv::Vec3b( 0, 255, 255 ); // Yellow
|
||
|
|
}
|
||
|
|
|
||
|
|
// Finalize
|
||
|
|
void Kinect::finalize()
|
||
|
|
{
|
||
|
|
cv::destroyAllWindows();
|
||
|
|
|
||
|
|
// Release Body Buffer
|
||
|
|
Concurrency::parallel_for_each( bodies.begin(), bodies.end(), []( IBody*& body ){
|
||
|
|
SafeRelease( body );
|
||
|
|
} );
|
||
|
|
|
||
|
|
// Close Sensor
|
||
|
|
if( kinect != nullptr ){
|
||
|
|
kinect->Close();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Update Data
|
||
|
|
void Kinect::update()
|
||
|
|
{
|
||
|
|
// Update Color
|
||
|
|
updateColor();
|
||
|
|
|
||
|
|
// Update Body
|
||
|
|
updateBody();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Update Color
|
||
|
|
inline void Kinect::updateColor()
|
||
|
|
{
|
||
|
|
// Retrieve Color Frame
|
||
|
|
ComPtr<IColorFrame> colorFrame;
|
||
|
|
const HRESULT ret = colorFrameReader->AcquireLatestFrame( &colorFrame );
|
||
|
|
if( FAILED( ret ) ){
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Convert Format ( YUY2 -> BGRA )
|
||
|
|
ERROR_CHECK( colorFrame->CopyConvertedFrameDataToArray( static_cast<UINT>( colorBuffer.size() ), &colorBuffer[0], ColorImageFormat::ColorImageFormat_Bgra ) );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Update Body
|
||
|
|
inline void Kinect::updateBody()
|
||
|
|
{
|
||
|
|
// Retrieve Body Frame
|
||
|
|
ComPtr<IBodyFrame> bodyFrame;
|
||
|
|
const HRESULT ret = bodyFrameReader->AcquireLatestFrame( &bodyFrame );
|
||
|
|
if( FAILED( ret ) ){
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Release Previous Bodies
|
||
|
|
Concurrency::parallel_for_each( bodies.begin(), bodies.end(), []( IBody*& body ){
|
||
|
|
SafeRelease( body );
|
||
|
|
} );
|
||
|
|
|
||
|
|
// Retrieve Body Data
|
||
|
|
ERROR_CHECK( bodyFrame->GetAndRefreshBodyData( BODY_COUNT, &bodies[0] ) );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Draw Data
|
||
|
|
void Kinect::draw()
|
||
|
|
{
|
||
|
|
// Draw Color
|
||
|
|
drawColor();
|
||
|
|
|
||
|
|
// Draw Body
|
||
|
|
drawBody();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Draw Color
|
||
|
|
inline void Kinect::drawColor()
|
||
|
|
{
|
||
|
|
// Create cv::Mat from Color Buffer
|
||
|
|
colorMat = cv::Mat( colorHeight, colorWidth, CV_8UC4, &colorBuffer[0] );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Draw Body
|
||
|
|
inline void Kinect::drawBody()
|
||
|
|
{
|
||
|
|
// Draw Body Data to Color Data
|
||
|
|
Concurrency::parallel_for( 0, BODY_COUNT, [&]( const int count ){
|
||
|
|
const ComPtr<IBody> body = bodies[count];
|
||
|
|
if( body == nullptr ){
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Check Body Tracked
|
||
|
|
BOOLEAN tracked = FALSE;
|
||
|
|
ERROR_CHECK( body->get_IsTracked( &tracked ) );
|
||
|
|
if( !tracked ){
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Retrieve Joints
|
||
|
|
std::array<Joint, JointType::JointType_Count> joints;
|
||
|
|
ERROR_CHECK( body->GetJoints( JointType::JointType_Count, &joints[0] ) );
|
||
|
|
|
||
|
|
#ifdef SMOOTH
|
||
|
|
// Update Joints
|
||
|
|
Sample::FilterDoubleExponential filter = filters[count];
|
||
|
|
filter.Update( &joints[0] );
|
||
|
|
|
||
|
|
// Retrive Filtered Joints
|
||
|
|
const DirectX::XMVECTOR* filteredJoints = filter.GetFilteredJoints();
|
||
|
|
#endif
|
||
|
|
|
||
|
|
Concurrency::parallel_for( 0, static_cast<int>( JointType::JointType_Count ), [&]( const int type ){
|
||
|
|
// Check Joint Tracked
|
||
|
|
Joint joint = joints[type];
|
||
|
|
if( joint.TrackingState == TrackingState::TrackingState_NotTracked ){
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef SMOOTH
|
||
|
|
// Retrive Filtered Joint
|
||
|
|
const DirectX::XMVECTOR filteredJoint = filteredJoints[type];
|
||
|
|
DirectX::XMVectorGetXPtr( &joint.Position.X, filteredJoint );
|
||
|
|
DirectX::XMVectorGetYPtr( &joint.Position.Y, filteredJoint );
|
||
|
|
DirectX::XMVectorGetZPtr( &joint.Position.Z, filteredJoint );
|
||
|
|
#endif
|
||
|
|
|
||
|
|
// Draw Joint Position
|
||
|
|
drawEllipse( colorMat, joint, 5, colors[count] );
|
||
|
|
|
||
|
|
// Draw Left Hand State
|
||
|
|
if( joint.JointType == JointType::JointType_HandLeft ){
|
||
|
|
HandState handState;
|
||
|
|
TrackingConfidence handConfidence;
|
||
|
|
ERROR_CHECK( body->get_HandLeftState( &handState ) );
|
||
|
|
ERROR_CHECK( body->get_HandLeftConfidence( &handConfidence ) );
|
||
|
|
|
||
|
|
drawHandState( colorMat, joint, handState, handConfidence );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Draw Right Hand State
|
||
|
|
if( joint.JointType == JointType::JointType_HandRight ){
|
||
|
|
HandState handState;
|
||
|
|
TrackingConfidence handConfidence;
|
||
|
|
ERROR_CHECK( body->get_HandRightState( &handState ) );
|
||
|
|
ERROR_CHECK( body->get_HandRightConfidence( &handConfidence ) );
|
||
|
|
|
||
|
|
drawHandState( colorMat, joint, handState, handConfidence );
|
||
|
|
}
|
||
|
|
} );
|
||
|
|
|
||
|
|
/*
|
||
|
|
// Retrieve Joint Orientations
|
||
|
|
std::array<JointOrientation, JointType::JointType_Count> orientations;
|
||
|
|
ERROR_CHECK( body->GetJointOrientations( JointType::JointType_Count, &orientations[0] ) );
|
||
|
|
*/
|
||
|
|
|
||
|
|
/*
|
||
|
|
// Retrieve Amount of Body Lean
|
||
|
|
PointF amount;
|
||
|
|
ERROR_CHECK( body->get_Lean( &amount ) );
|
||
|
|
*/
|
||
|
|
} );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Draw Ellipse
|
||
|
|
inline void Kinect::drawEllipse( cv::Mat& image, const Joint& joint, const int radius, const cv::Vec3b& color, const int thickness )
|
||
|
|
{
|
||
|
|
if( image.empty() ){
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Convert Coordinate System and Draw Joint
|
||
|
|
ColorSpacePoint colorSpacePoint;
|
||
|
|
ERROR_CHECK( coordinateMapper->MapCameraPointToColorSpace( joint.Position, &colorSpacePoint ) );
|
||
|
|
const int x = static_cast<int>( colorSpacePoint.X + 0.5f );
|
||
|
|
const int y = static_cast<int>( colorSpacePoint.Y + 0.5f );
|
||
|
|
if( ( 0 <= x ) && ( x < image.cols ) && ( 0 <= y ) && ( y < image.rows ) ){
|
||
|
|
cv::circle( image, cv::Point( x, y ), radius, static_cast<cv::Scalar>( color ), thickness, cv::LINE_AA );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Draw Hand State
|
||
|
|
inline void Kinect::drawHandState( cv::Mat& image, const Joint& joint, HandState handState, TrackingConfidence handConfidence )
|
||
|
|
{
|
||
|
|
if( image.empty() ){
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Check Tracking Confidence
|
||
|
|
if( handConfidence != TrackingConfidence::TrackingConfidence_High ){
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Draw Hand State
|
||
|
|
const int radius = 75;
|
||
|
|
const cv::Vec3b blue = cv::Vec3b( 128, 0, 0 ), green = cv::Vec3b( 0, 128, 0 ), red = cv::Vec3b( 0, 0, 128 );
|
||
|
|
switch( handState ){
|
||
|
|
// Open
|
||
|
|
case HandState::HandState_Open:
|
||
|
|
drawEllipse( image, joint, radius, green, 5 );
|
||
|
|
break;
|
||
|
|
// Close
|
||
|
|
case HandState::HandState_Closed:
|
||
|
|
drawEllipse( image, joint, radius, red, 5 );
|
||
|
|
break;
|
||
|
|
// Lasso
|
||
|
|
case HandState::HandState_Lasso:
|
||
|
|
drawEllipse( image, joint, radius, blue, 5 );
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Show Data
|
||
|
|
void Kinect::show()
|
||
|
|
{
|
||
|
|
// Show Body
|
||
|
|
showBody();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Show Body
|
||
|
|
inline void Kinect::showBody()
|
||
|
|
{
|
||
|
|
if( colorMat.empty() ){
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Resize Image
|
||
|
|
cv::Mat resizeMat;
|
||
|
|
const double scale = 0.5;
|
||
|
|
cv::resize( colorMat, resizeMat, cv::Size(), scale, scale );
|
||
|
|
|
||
|
|
// Show Image
|
||
|
|
cv::imshow( "JointSmooth", resizeMat );
|
||
|
|
}
|