direct access to rawSkeletonPoints

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

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.

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<BYTE> 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