From 552919d2daed3c209ce94b6318274aa52c1dd6b6 Mon Sep 17 00:00:00 2001 From: frankiezafe Date: Wed, 5 Nov 2014 14:08:59 +0000 Subject: [PATCH 1/7] direct access to rawSkeletonPoints enabling a fast and efficient skeletons data retrieval --- src/ofxKinectNui.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/ofxKinectNui.h b/src/ofxKinectNui.h index a756b20..b046473 100644 --- a/src/ofxKinectNui.h +++ b/src/ofxKinectNui.h @@ -149,6 +149,16 @@ class ofxKinectNui: public ofxBase3DVideo{ int getSkeletonPoints(ofPoint* ret[]); int getRawSkeletonPoints(ofPoint* ret[]); + // allows direct retrieval of the raw points, without copying + // to use, use something like this: + // const ofPoint* rawpts = kinect.getRaw_ptr(); + // for ( int k = 0; k < ofxKinectNui::SKELETON_COUNT; k++ ) { + // // jumping to the next skeleton's points >> k * ofxKinectNui::SKELETON_POSITION_COUNT + // const ofPoint* pt = ( rawpts + ( k * ofxKinectNui::SKELETON_POSITION_COUNT + 0 )); + // and so on... + const ofPoint* getRaw_ptr() const { return &rawSkeletonPoints[0][0]; } + + ofColor getColorAt(int x, int y); ofColor getColorAt(const ofPoint& point); From 988b30747ea54f599d77da6777916b68ba4da56f Mon Sep 17 00:00:00 2001 From: frankiezafe Date: Wed, 5 Nov 2014 20:54:42 +0000 Subject: [PATCH 2/7] confidence adding confidence retrieval, custom method (0: not tracked, 0.1: tracked but poor quality, [0.25, 1]: tracked, vary depending on the distance, closer being better) --- src/kinect/nui/SkeletonFrame.h | 4 +++ src/ofxKinectNui.cpp | 61 +++++++++++++++++++++++++++++----- src/ofxKinectNui.h | 18 ++++------ 3 files changed, 64 insertions(+), 19 deletions(-) diff --git a/src/kinect/nui/SkeletonFrame.h b/src/kinect/nui/SkeletonFrame.h index ba5b1d2..8452779 100644 --- a/src/kinect/nui/SkeletonFrame.h +++ b/src/kinect/nui/SkeletonFrame.h @@ -86,6 +86,10 @@ namespace kinect { return skeletonData_.SkeletonPositions[index]; } + NUI_SKELETON_POSITION_TRACKING_STATE getConfidence( int index ) { + return skeletonData_.eSkeletonPositionTrackingState[ index ]; + } + //---------------------------------------------------------- /** @brief Get user index diff --git a/src/ofxKinectNui.cpp b/src/ofxKinectNui.cpp index 107c1c8..c31a124 100644 --- a/src/ofxKinectNui.cpp +++ b/src/ofxKinectNui.cpp @@ -42,6 +42,7 @@ ofxKinectNui::ofxKinectNui(){ skeletonDraw_ = NULL; addKinectListener(this, &ofxKinectNui::pluggedFunc, &ofxKinectNui::unpluggedFunc); + } //--------------------------------------------------------------------------- @@ -376,6 +377,7 @@ void ofxKinectNui::close(){ @brief update stream data. */ void ofxKinectNui::update(UINT flag){ + if(!kinect.IsInited() || !kinect.IsConnected() || !isOpened()){ return; } @@ -514,44 +516,87 @@ void ofxKinectNui::update(UINT flag){ } if(flag & UPDATE_FLAG_GROUP_SKELETON){ + // Get the skeleton data of next frame kinect::nui::SkeletonFrame skeleton = kinect.Skeleton().GetNextFrame(30); - if(skeleton.IsFoundSkeleton()){ + + if( skeleton.IsFoundSkeleton() ){ + bIsFoundSkeleton = true; skeleton.TransformSmooth(); + for(int i = 0; i < kinect::nui::SkeletonFrame::SKELETON_COUNT; ++i){ - if( skeleton[i].TrackingState() == NUI_SKELETON_TRACKED){ + + if( skeleton[i].TrackingState() == NUI_SKELETON_TRACKED ){ + for(int j = 0; j < kinect::nui::SkeletonData::POSITION_COUNT; ++j){ + kinect::nui::SkeletonData::SkeletonPoint p = skeleton[i].TransformSkeletonToDepthImage(j, mDepthResolution); - skeletonPoints[i][j] = ofPoint(bIsMirror ? p.x : depthWidth - 1 - p.x, p.y, p.depth); - rawSkeletonPoints[i][j] = ofPoint(bIsMirror ? skeleton[i][j].x : -skeleton[i][j].x, skeleton[i][j].y, skeleton[i][j].z); + skeletonPoints[i][j] = ofPoint( bIsMirror ? p.x : depthWidth - 1 - p.x, p.y, p.depth ); + rawSkeletonPoints[i][j] = ofPoint( bIsMirror ? skeleton[i][j].x : -skeleton[i][j].x, skeleton[i][j].y, skeleton[i][j].z ); + + // OpenNI style confidence + skeletonConfidence[i][j] = 0; + float d; + + switch( skeleton[i].getConfidence( j ) ) { + + case NUI_SKELETON_POSITION_TRACKED: + // closer => better + // 1m => 1 + // 4.5m => 0.25 + d = ( ( abs( skeleton[i][j].z ) - 1.f ) / 3.5f ); + if ( d < 0 ) { + d = 0; + } else if ( d > 1 ) { + d = 1; + } + skeletonConfidence[ i ][ j ] = 0.15f + ( 1 - d ) * 0.85f; + break; + + case NUI_SKELETON_POSITION_INFERRED: + skeletonConfidence[ i ][ j ] = 0.1f; + break; + + default: + break; + + } + } - }else{ + + } else { + // if skeleton is not tracked, set top z data negative. skeletonPoints[i][0].z = -1; rawSkeletonPoints[i][0].z = -1; continue; + } + } - } - else { + } else { + bIsFoundSkeleton = false; for(int i = 0; i < kinect::nui::SkeletonFrame::SKELETON_COUNT; ++i){ // if skeleton is not tracked, set top z data negative. skeletonPoints[i][0].z = -1; rawSkeletonPoints[i][0].z = -1; } + } - }else{ + } else { cout << "false2" << endl; bIsFoundSkeleton = false; } if(flag & UPDATE_FLAG_GROUP_AUDIO){ + soundBuffer = kinect.AudioStream().Read(); audioBeamAngle = (float)kinect.AudioStream().GetAudioBeamAngle(); audioAngle = (float)kinect.AudioStream().GetAudioAngle(); audioAngleConfidence = (float)kinect.AudioStream().GetAudioAngleConfidence(); + } bIsFrameNew = true; diff --git a/src/ofxKinectNui.h b/src/ofxKinectNui.h index b046473..a3d6d6e 100644 --- a/src/ofxKinectNui.h +++ b/src/ofxKinectNui.h @@ -35,6 +35,7 @@ class IDrawPoints; /****************************************/ class ofxKinectNui: public ofxBase3DVideo{ public: + ofxKinectNui(); virtual ~ofxKinectNui(); @@ -146,18 +147,10 @@ class ofxKinectNui: public ofxBase3DVideo{ ofShortPixels& getDistancePixels(); std::vector getSoundBuffer(); - int getSkeletonPoints(ofPoint* ret[]); - int getRawSkeletonPoints(ofPoint* ret[]); - - // allows direct retrieval of the raw points, without copying - // to use, use something like this: - // const ofPoint* rawpts = kinect.getRaw_ptr(); - // for ( int k = 0; k < ofxKinectNui::SKELETON_COUNT; k++ ) { - // // jumping to the next skeleton's points >> k * ofxKinectNui::SKELETON_POSITION_COUNT - // const ofPoint* pt = ( rawpts + ( k * ofxKinectNui::SKELETON_POSITION_COUNT + 0 )); - // and so on... + int getSkeletonPoints( ofPoint* ret[] ); + int getRawSkeletonPoints( ofPoint* ret[] ); const ofPoint* getRaw_ptr() const { return &rawSkeletonPoints[0][0]; } - + const float getConfidenceAt( int i, int j ) const { return skeletonConfidence[ i ][ j ]; } ofColor getColorAt(int x, int y); ofColor getColorAt(const ofPoint& point); @@ -234,6 +227,7 @@ class ofxKinectNui: public ofxBase3DVideo{ } protected: + kinect::nui::Kinect kinect; ///< kinect instance ofPixels videoPixels; ///< video pixels @@ -247,6 +241,7 @@ class ofxKinectNui: public ofxBase3DVideo{ ofPoint skeletonPoints[SKELETON_COUNT][SKELETON_POSITION_COUNT]; ///< joint points of all skeletons ofPoint rawSkeletonPoints[SKELETON_COUNT][SKELETON_POSITION_COUNT]; ///< joint points of all skeletons + float skeletonConfidence[SKELETON_COUNT][SKELETON_POSITION_COUNT]; ///< openni style confidence int targetAngle; ///< target angle of kinect tilt @@ -275,5 +270,6 @@ class ofxKinectNui: public ofxBase3DVideo{ IDrawPixels* depthDraw_; IDrawPixels* labelDraw_; IDrawPoints* skeletonDraw_; + }; #endif // OFX_KINECT_NUI_H \ No newline at end of file From 30fc5982a3411615897a66f571ccd411ccde033e Mon Sep 17 00:00:00 2001 From: frankiezafe Date: Mon, 1 Dec 2014 12:16:27 +0100 Subject: [PATCH 3/7] better confidence > lower if near the kinect field of view limits --- src/ofxKinectNui.cpp | 105 +++++++++++++++++++++++++++++++------------ src/ofxKinectNui.h | 13 ++++++ 2 files changed, 90 insertions(+), 28 deletions(-) diff --git a/src/ofxKinectNui.cpp b/src/ofxKinectNui.cpp index c31a124..ae69c8d 100644 --- a/src/ofxKinectNui.cpp +++ b/src/ofxKinectNui.cpp @@ -43,6 +43,10 @@ ofxKinectNui::ofxKinectNui(){ addKinectListener(this, &ofxKinectNui::pluggedFunc, &ofxKinectNui::unpluggedFunc); + fov_limit = FOV_LIMIT_DISTANCE; + fov_half_h = NUI_CAMERA_DEPTH_NOMINAL_HORIZONTAL_FOV * 0.5f; + fov_half_v = NUI_CAMERA_DEPTH_NOMINAL_VERTICAL_FOV * 0.5f; + } //--------------------------------------------------------------------------- @@ -534,34 +538,8 @@ void ofxKinectNui::update(UINT flag){ kinect::nui::SkeletonData::SkeletonPoint p = skeleton[i].TransformSkeletonToDepthImage(j, mDepthResolution); skeletonPoints[i][j] = ofPoint( bIsMirror ? p.x : depthWidth - 1 - p.x, p.y, p.depth ); rawSkeletonPoints[i][j] = ofPoint( bIsMirror ? skeleton[i][j].x : -skeleton[i][j].x, skeleton[i][j].y, skeleton[i][j].z ); - - // OpenNI style confidence - skeletonConfidence[i][j] = 0; - float d; - - switch( skeleton[i].getConfidence( j ) ) { - - case NUI_SKELETON_POSITION_TRACKED: - // closer => better - // 1m => 1 - // 4.5m => 0.25 - d = ( ( abs( skeleton[i][j].z ) - 1.f ) / 3.5f ); - if ( d < 0 ) { - d = 0; - } else if ( d > 1 ) { - d = 1; - } - skeletonConfidence[ i ][ j ] = 0.15f + ( 1 - d ) * 0.85f; - break; - - case NUI_SKELETON_POSITION_INFERRED: - skeletonConfidence[ i ][ j ] = 0.1f; - break; - - default: - break; - - } + skeletonConfidence[i][j] = processConfidence( &skeleton[i], &rawSkeletonPoints[i][j], i, j ); + continue; } @@ -570,6 +548,7 @@ void ofxKinectNui::update(UINT flag){ // if skeleton is not tracked, set top z data negative. skeletonPoints[i][0].z = -1; rawSkeletonPoints[i][0].z = -1; + skeletonConfidence[i][0] = 0; continue; } @@ -582,6 +561,7 @@ void ofxKinectNui::update(UINT flag){ // if skeleton is not tracked, set top z data negative. skeletonPoints[i][0].z = -1; rawSkeletonPoints[i][0].z = -1; + skeletonConfidence[i][0] = 0; } } @@ -602,6 +582,75 @@ void ofxKinectNui::update(UINT flag){ bIsFrameNew = true; } +float ofxKinectNui::processConfidence( kinect::nui::SkeletonData* skData, ofPoint* rawp, int skID, int rawpID ) { + + if ( rawp->z == 0 ) { + return 0; + } + + float conf = 0; + float d; + + switch( skData->getConfidence( rawpID ) ) { + case NUI_SKELETON_POSITION_TRACKED: + // closer => better + // 1.5m => 1 + // 4.5m => 0.15 + d = ( ( abs( rawp->z ) - 1.5f ) / 3.f ); + if ( d < 0 ) { + d = 0; + } else if ( d > 1 ) { + d = 1; + } + conf = 0.15f + ( 1 - d ) * 0.85f; + break; + case NUI_SKELETON_POSITION_INFERRED: + conf = 0.1f; + break; + default: + return 0; + } + + // second pass: + // if the point is too close from the FOV, + // confidence drops down + float anglh = atan2f( rawp->x, rawp->z ); + float anglv = atan2f( rawp->y, rawp->x ); + float ratioh = 1; + float ratiov = 1; + if ( + ( anglh < 0 && anglh - -fov_half_h < fov_limit ) || + ( anglh > 0 && fov_half_h - anglh < fov_limit ) + ) { + if ( anglh < 0 ) { + ratioh = ( anglh + fov_half_h ) / fov_limit; + } + if ( anglh > 0 ) { + ratioh = ( fov_half_h - anglh ) / fov_limit; + } + } + if ( + ( anglv < 0 && anglv - -fov_half_v < fov_limit ) || + ( anglv > 0 && fov_half_v - anglv < fov_limit ) + ) { + if ( anglv < 0 ) { + ratiov = ( anglv + fov_half_v ) / fov_limit; + } + if ( anglv > 0 ) { + ratiov = ( fov_half_v - anglv ) / fov_limit; + } + } + + // never more than / 10 + if ( ratioh <= 0 || ratiov <= 0 ) { + return conf * 0.1f; + } else { + return conf * ( 0.1f + ( ratioh * ratiov ) * 0.9f ) ; + } + + +} + //--------------------------------------------------------------------------- /** @brief listener function when kinect is plugged. diff --git a/src/ofxKinectNui.h b/src/ofxKinectNui.h index a3d6d6e..ec9ac84 100644 --- a/src/ofxKinectNui.h +++ b/src/ofxKinectNui.h @@ -18,6 +18,8 @@ #include "ofMain.h" #include "ofxBase3DVideo.h" +#define FOV_LIMIT_DISTANCE 0.08726647f // 5 degree in radian + class IDrawPixels; class IDrawPoints; @@ -209,7 +211,12 @@ class ofxKinectNui: public ofxBase3DVideo{ kinect.RemoveKinectListener(object); } + // confidence related + void setFovLimit( float l ) { fov_limit = l; } + float getFovLimit() { return fov_limit; } + public: + const static int KINECT_PLAYERS_INDEX_NUM = 8; static int getActiveCount(){ @@ -271,5 +278,11 @@ class ofxKinectNui: public ofxBase3DVideo{ IDrawPixels* labelDraw_; IDrawPoints* skeletonDraw_; + // confidence related + float fov_limit; + float fov_half_h; + float fov_half_v; + float processConfidence( kinect::nui::SkeletonData* skData, ofPoint* rawp, int skID, int rawpID ); + }; #endif // OFX_KINECT_NUI_H \ No newline at end of file From 2199a4bf375cb5b252503f1fb25d1cd3ce6210d0 Mon Sep 17 00:00:00 2001 From: frankiezafe Date: Mon, 1 Dec 2014 12:35:53 +0100 Subject: [PATCH 4/7] minor change --- src/ofxKinectNui.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/ofxKinectNui.cpp b/src/ofxKinectNui.cpp index ae69c8d..3d50b1e 100644 --- a/src/ofxKinectNui.cpp +++ b/src/ofxKinectNui.cpp @@ -589,20 +589,21 @@ float ofxKinectNui::processConfidence( kinect::nui::SkeletonData* skData, ofPoin } float conf = 0; - float d; switch( skData->getConfidence( rawpID ) ) { case NUI_SKELETON_POSITION_TRACKED: // closer => better // 1.5m => 1 // 4.5m => 0.15 - d = ( ( abs( rawp->z ) - 1.5f ) / 3.f ); - if ( d < 0 ) { - d = 0; - } else if ( d > 1 ) { - d = 1; + { + float d = ( ( abs( rawp->z ) - 1.5f ) / 3.f ); + if ( d < 0 ) { + d = 0; + } else if ( d > 1 ) { + d = 1; + } + conf = 0.15f + ( 1 - d ) * 0.85f; } - conf = 0.15f + ( 1 - d ) * 0.85f; break; case NUI_SKELETON_POSITION_INFERRED: conf = 0.1f; From 167c608a3f96267fcb57ddc116e9fd14e44d5ea4 Mon Sep 17 00:00:00 2001 From: frankiezafe Date: Mon, 1 Dec 2014 13:13:50 +0100 Subject: [PATCH 5/7] degree to radian conversion in objet constructor --- src/ofxKinectNui.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ofxKinectNui.cpp b/src/ofxKinectNui.cpp index 3d50b1e..3496340 100644 --- a/src/ofxKinectNui.cpp +++ b/src/ofxKinectNui.cpp @@ -44,8 +44,8 @@ ofxKinectNui::ofxKinectNui(){ addKinectListener(this, &ofxKinectNui::pluggedFunc, &ofxKinectNui::unpluggedFunc); fov_limit = FOV_LIMIT_DISTANCE; - fov_half_h = NUI_CAMERA_DEPTH_NOMINAL_HORIZONTAL_FOV * 0.5f; - fov_half_v = NUI_CAMERA_DEPTH_NOMINAL_VERTICAL_FOV * 0.5f; + fov_half_h = ( NUI_CAMERA_DEPTH_NOMINAL_HORIZONTAL_FOV * 0.5f ) / 180.f * PI; + fov_half_v = ( NUI_CAMERA_DEPTH_NOMINAL_VERTICAL_FOV * 0.5f ) / 180.f * PI; } From 69b4cf9eccfe20200f4807077362ae94072d5eb5 Mon Sep 17 00:00:00 2001 From: frankiezafe Date: Mon, 1 Dec 2014 14:43:16 +0100 Subject: [PATCH 6/7] getter on half FOV vertical & horizontal --- src/ofxKinectNui.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/ofxKinectNui.h b/src/ofxKinectNui.h index ec9ac84..7a3f314 100644 --- a/src/ofxKinectNui.h +++ b/src/ofxKinectNui.h @@ -214,6 +214,8 @@ class ofxKinectNui: public ofxBase3DVideo{ // confidence related void setFovLimit( float l ) { fov_limit = l; } float getFovLimit() { return fov_limit; } + float getFovHalfHorizontal() { return fov_half_h; } + float getFovHalfVertical() { return fov_half_v; } public: From e4ed4d384fc8dbac38fd14bcf9501fcd51f29860 Mon Sep 17 00:00:00 2001 From: frankiezafe Date: Mon, 1 Dec 2014 15:20:17 +0100 Subject: [PATCH 7/7] correction --- src/ofxKinectNui.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ofxKinectNui.cpp b/src/ofxKinectNui.cpp index 3496340..689512f 100644 --- a/src/ofxKinectNui.cpp +++ b/src/ofxKinectNui.cpp @@ -616,7 +616,7 @@ float ofxKinectNui::processConfidence( kinect::nui::SkeletonData* skData, ofPoin // if the point is too close from the FOV, // confidence drops down float anglh = atan2f( rawp->x, rawp->z ); - float anglv = atan2f( rawp->y, rawp->x ); + float anglv = atan2f( rawp->y, rawp->z ); float ratioh = 1; float ratiov = 1; if (