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..689512f 100644 --- a/src/ofxKinectNui.cpp +++ b/src/ofxKinectNui.cpp @@ -42,6 +42,11 @@ ofxKinectNui::ofxKinectNui(){ skeletonDraw_ = NULL; addKinectListener(this, &ofxKinectNui::pluggedFunc, &ofxKinectNui::unpluggedFunc); + + fov_limit = FOV_LIMIT_DISTANCE; + 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; + } //--------------------------------------------------------------------------- @@ -376,6 +381,7 @@ void ofxKinectNui::close(){ @brief update stream data. */ void ofxKinectNui::update(UINT flag){ + if(!kinect.IsInited() || !kinect.IsConnected() || !isOpened()){ return; } @@ -514,49 +520,138 @@ 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 ); + skeletonConfidence[i][j] = processConfidence( &skeleton[i], &rawSkeletonPoints[i][j], i, j ); + continue; + } - }else{ + + } else { + // 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; + } + } - } - 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; + skeletonConfidence[i][0] = 0; } + } - }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; } +float ofxKinectNui::processConfidence( kinect::nui::SkeletonData* skData, ofPoint* rawp, int skID, int rawpID ) { + + if ( rawp->z == 0 ) { + return 0; + } + + float conf = 0; + + switch( skData->getConfidence( rawpID ) ) { + case NUI_SKELETON_POSITION_TRACKED: + // closer => better + // 1.5m => 1 + // 4.5m => 0.15 + { + 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; + } + 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->z ); + 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 a756b20..7a3f314 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; @@ -35,6 +37,7 @@ class IDrawPoints; /****************************************/ class ofxKinectNui: public ofxBase3DVideo{ public: + ofxKinectNui(); virtual ~ofxKinectNui(); @@ -146,8 +149,10 @@ class ofxKinectNui: public ofxBase3DVideo{ ofShortPixels& getDistancePixels(); std::vector getSoundBuffer(); - int getSkeletonPoints(ofPoint* ret[]); - int getRawSkeletonPoints(ofPoint* ret[]); + 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); @@ -206,7 +211,14 @@ class ofxKinectNui: public ofxBase3DVideo{ kinect.RemoveKinectListener(object); } + // 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: + const static int KINECT_PLAYERS_INDEX_NUM = 8; static int getActiveCount(){ @@ -224,6 +236,7 @@ class ofxKinectNui: public ofxBase3DVideo{ } protected: + kinect::nui::Kinect kinect; ///< kinect instance ofPixels videoPixels; ///< video pixels @@ -237,6 +250,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 @@ -265,5 +279,12 @@ class ofxKinectNui: public ofxBase3DVideo{ IDrawPixels* depthDraw_; 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