Commits

schlangen committed 970fa90 Draft

- improved alignment using gravity sensor
- reduced deph noise / skeleton joint flickering
- improved gesture definition

  • Participants
  • Parent commits 256c54b

Comments (0)

Files changed (2)

     TILT_LIBS := -lusb-1.0 -lfreespace
 else
 ifeq ($(ENABLE_TILT),kinect)
-    TILT_LIBS := -lOpenNI -lXnVNite_1_5_2
-    ALL_CPPFLAGS += -I/usr/include/ni
+    TILT_LIBS := -lOpenNI -lXnVNite_1_5_2 -lfreenect
+    ALL_CPPFLAGS += -I/usr/include/ni -I/usr/local/include/libfreenect
 endif
 endif
 endif 

share/tilt_kinect.c

 
 #include <SDL.h>
 #include <SDL_thread.h>
+
 #include <math.h>
 #include <stdio.h>
 
 
 #include <XnOpenNI.h>
 
-
+#include "libfreenect.h"
 
 // openni variables
 
 // caches all joint coords
 static float allJointCoords[24][3];
 
+static float refx=0, refy=0, refz=0;
+
 static int startedTracking = 0;
 
+freenect_context *f_ctx;
+freenect_device *f_dev;
+
 //  New user
 void XN_CALLBACK_TYPE new_user(XnNodeHandle generator,
 			       XnUserID nId, 
 	if (status != XN_CALIBRATION_STATUS_NO_USER) {
 		printf("Calibration complete, start tracking user %d\n", nId);
 		xnStartSkeletonTracking(capability, nId);
+		refx = 0;refy=0;refz=0;
 		startedTracking = 1;
 	}
 	else {
 		    jointPos(aUsers[0], (XnSkeletonJoint) j);
 		  }		
 		}
+		
+		if (refx == 0){
+		  refx = allJointCoords[XN_SKEL_TORSO-1][0];
+		  refy = allJointCoords[XN_SKEL_TORSO-1][1];
+		  refz = allJointCoords[XN_SKEL_TORSO-1][2];
+		}
 	}
+	
 	free(aUsers);
 }
 
 void init_kinect() {
 	printf("Initializing...\n");
 	
+	printf("init freenect:\n");
+	
+	 if (freenect_init(&f_ctx, NULL) < 0) {
+	      printf("freenect_init() failed\n");
+	      return;
+	  }
+	  printf("only motor\n");
+	  freenect_select_subdevices(f_ctx, FREENECT_DEVICE_MOTOR); // 0x01
+	  printf("open dev\n");
+	  if (freenect_open_device(f_ctx, &f_dev, 0) < 0) {
+	      printf("Could not open device\n");
+	      return;
+	  }
+	
+	printf("init opneni:\n");
+	
 	XnStatus nRetVal = XN_STATUS_OK;
 	XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
 
   xnContextRelease(pContext);
 }
 
+double gravangle = -1000;
+double gravangle2 = -1000;
+double dummy = 0;
+double test = 0;
+double test2 = 0;
+
 static int tilt_func(void *data)
 {
        printf("hello kinect world! inited!");
     SDL_mutexP(mutex);
     state.status = running;
     SDL_mutexV(mutex);
-
+  // int dummy=0;
     while (mutex && running)
     {
         SDL_mutexP(mutex);
             {
 	      if (startedTracking){
              //float deltaX, deltaZ;
-              float a,b,c, x1,x2,y1,y2,z1,z2;
+           //   float a,b,c, x1,x2,y1,y2,z1,z2;
   
 	      // difference in mm from neck to waist
 	//      deltaX = (allJointCoords[XN_SKEL_NECK-1][0] - allJointCoords[XN_SKEL_WAIST-1][0]) / 20;//200;
 	//      deltaZ = (allJointCoords[XN_SKEL_NECK-1][2] - allJointCoords[XN_SKEL_WAIST-1][2] - 10) / 5;//100;
 	     
+	      if (refx != 0){
 	      
-	      x1 = allJointCoords[XN_SKEL_LEFT_HAND-1][0];
-	      y1 = allJointCoords[XN_SKEL_LEFT_HAND-1][1];
-	      z1 = allJointCoords[XN_SKEL_LEFT_HAND-1][2];
-	      x2 = allJointCoords[XN_SKEL_LEFT_ELBOW-1][0];
-	      y2 = allJointCoords[XN_SKEL_LEFT_ELBOW-1][1];
-	      z2 = allJointCoords[XN_SKEL_LEFT_ELBOW-1][2];
-	      
-	      a = x2 - x1;
-	      b = y2 - y1;
-	      c = sqrt(a*a + b*b);
-	      state.z = asin(a/c) * 180 / PI;
-	      // avoid nan values
-	      if (state.x!= state.x) state.x = 0;
-	      
-	      a = z2 - z1;
-	      c = sqrt(a*a + b*b);
-	      state.x = asin(a/c) * 180 / PI;
-	      // avoid nan values
-	      if (state.z != state.z) state.z = 0;
+		  /*x1 = allJointCoords[XN_SKEL_LEFT_HAND-1][0];
+		  y1 = allJointCoords[XN_SKEL_LEFT_HAND-1][1];
+		  z1 = allJointCoords[XN_SKEL_LEFT_HAND-1][2];*/
+	/*	  x1 = allJointCoords[XN_SKEL_HEAD-1][0];
+		  y1 = allJointCoords[XN_SKEL_HEAD-1][1];
+		  z1 = allJointCoords[XN_SKEL_HEAD-1][2];
+	*/	
+		 /* x1 = refx;
+		  y1 = refy;
+		  z1 = refz;*/
+		  
+	/*	  x2 = allJointCoords[XN_SKEL_WAIST-1][0];
+		  y2 = allJointCoords[XN_SKEL_WAIST-1][1];
+		  z2 = allJointCoords[XN_SKEL_WAIST-1][2];
+		  
+		  a = x2 - x1;
+		  b = y2 - y1;
+		  c = sqrt(a*a + b*b);
+		  state.z = asin(a/c) * 180 / PI;
+		  // avoid nan values
+		  if (state.x!= state.x) state.x = 0;
+		  
+		  a = z2 - z1;
+		  c = sqrt(a*a + b*b);
+		  state.x = asin(a/c) * 180 / PI;
+		  // avoid nan values
+		  if (state.z != state.z) state.z = 0;
+	*/	  
+	
+		  float shoulder_height_l = allJointCoords[XN_SKEL_LEFT_SHOULDER-1][1]; 
+		  float shoulder_x_l = allJointCoords[XN_SKEL_LEFT_SHOULDER-1][0];
+		  float shoulder_height_r = allJointCoords[XN_SKEL_RIGHT_SHOULDER-1][1];
+		  float shoulder_x_r = allJointCoords[XN_SKEL_RIGHT_SHOULDER-1][0];
+		  
+		  float head_height = allJointCoords[XN_SKEL_HEAD-1][1]; 
+		  float head_z = allJointCoords[XN_SKEL_HEAD-1][2];
+		  //float waist_height = allJointCoords[XN_SKEL_WAIST-1][1];
+		  //float waist_z = allJointCoords[XN_SKEL_WAIST-1][2];
+		  //float waist_height = allJointCoords[XN_SKEL_TORSO-1][1];
+		  //float waist_z = allJointCoords[XN_SKEL_TORSO-1][2];
+		  float waist_height = allJointCoords[XN_SKEL_LEFT_HAND-1][1];
+		  float waist_z = allJointCoords[XN_SKEL_LEFT_HAND-1][2];
+		  
+		  float angle1 = atan2(shoulder_height_l - shoulder_height_r, shoulder_x_l-shoulder_x_r) * 180 / PI;
+		  
+		  //float angle3 = atan2(head_height - waist_height, head_z - waist_z) * 180 / PI;
+		  float angle3 = atan2(head_z - waist_z, head_height - waist_height) * 180 / PI;
+		  
+		  angle1 = angle1 < 0 ? (-180 -angle1) : (180 - angle1);
+		  angle3 = angle3 < 0 ? (-180 -angle3) : (180 - angle3);
+		  
+		  float angle2 = angle1;
+		  
+		  freenect_raw_tilt_state *tilt_state = 0;
+		  double dx, dy, dz;
+		    
+		  if (abs(gravangle) > 30){
+		  
+		    printf("update grav, curr at %f ", gravangle);
+		    // Get the raw accelerometer values and tilt data
+		    //if (freenect_sync_get_tilt_state(&state, 0)) {
+		      //freenect_sync_get_tilt_state(&state, 0);
+		      freenect_update_tilt_state(f_dev);
+		      tilt_state = freenect_get_tilt_state(f_dev);
+		      
+		      // Get the processed accelerometer values (calibrated to gravity)
+		      freenect_get_mks_accel(tilt_state, &dx, &dy, &dz);
+		      //freenect_sync_stop(); 
+		      gravangle = 90 - atan2(dy, dx) * 180 / PI;
+		      gravangle2 = 90 - atan2(dy, dz) * 180 / PI;
+		  }  
+		    angle2 = (angle2 -gravangle); //(int)(angle2 -gravangle);
+		    
+		    // use average *or* int cut-off here (or both?), add switch in options (best in-game, otherwise as compile flag)
+		    
+		    test = (2*test + angle2) / 3;
+		    test2 = (2*test2 - (angle3-gravangle2)) / 3;
+	
+		    
+		  state.z = test;
+		  // avoid nan values
+		  if (state.x!= state.x) state.x = 0;
+		  
+		  state.x = test2;
+		  // avoid nan values
+		  if (state.z != state.z) state.z = 0;
+		  
+	
+	      }
 	     
 	 //    printf("dist: %f\n", (allJointCoords[XN_SKEL_RIGHT_SHOULDER][2] - allJointCoords[XN_SKEL_RIGHT_HAND][2]));
 	     
 	      if (allJointCoords[XN_SKEL_RIGHT_HAND][1] < allJointCoords[XN_SKEL_HEAD][1]){  // small y -> high object, ie if hand.y < head.y -> hand is above head
 		if (allJointCoords[XN_SKEL_RIGHT_HAND][0] < allJointCoords[XN_SKEL_HEAD][0]){
 		  button = 1;
-		   
 		  //printf("button: 1!");
 		}else if (allJointCoords[XN_SKEL_RIGHT_HAND][1] < allJointCoords[XN_SKEL_HEAD][1]){
 		  button = 2;
 		}		
 	      }
 		
-	      if (allJointCoords[XN_SKEL_RIGHT_SHOULDER][2] - allJointCoords[XN_SKEL_RIGHT_HAND][2] > 250){
+       /*     if (allJointCoords[XN_SKEL_LEFT_SHOULDER][2] - allJointCoords[XN_SKEL_LEFT_HAND][2] > 250){
+		//printf("set button a\n");
+		printf("%.2f %.2f %.2f -> %.2f %.2f %.2f\n", x1, y1, z1, x2, y2, z2);
+		set_button(&state.A, 1);
+	      }
+	 */     
+		
+		
+	  /*    if (allJointCoords[XN_SKEL_RIGHT_SHOULDER][2] - allJointCoords[XN_SKEL_RIGHT_HAND][2] > 250){
 		x1 = allJointCoords[XN_SKEL_RIGHT_HAND-1][0];
 		y1 = allJointCoords[XN_SKEL_RIGHT_HAND-1][1];	      
 		x2 = allJointCoords[XN_SKEL_RIGHT_ELBOW-1][0];
 		    set_button(&state.D, 1);
 		  } 
 	        }
-	      }
+	      }*/
 	      
 	   //   printf("delta x: %f   delta y : %f\n", (x1-x2), (y1-y2));
 	      
 	      
 	      
-	      if (allJointCoords[XN_SKEL_LEFT_SHOULDER][2] - allJointCoords[XN_SKEL_LEFT_HAND][2] > 250){
-		printf("set button a\n");
-		set_button(&state.A, 1);
-	      }
-	      
+	  
             //    state.x = -deltaZ;
             //    state.z = -deltaX;