Kinect Joint Tracking

joint-tracking.png (575×324)

This program is similar to the basic skeleton tracking but also extracts position datas is he. This program will display the angle of all 4 major joints in your body in radians. This is helpful for robotic applications.

import SimpleOpenNI.*;
 
/*---------------------------------------------------------------
Variables
----------------------------------------------------------------*/
// create kinect object
SimpleOpenNI kinect;

// image storage from kinect
PImage kinectDepth;

// int of each user being tracked
int[] userID;

// user colors
color[] userColor = new color[]{ 
	color(255,0,0), 
	color(0,255,0), 
	color(0,0,255),
	color(255,255,0), 
	color(255,0,255), 
	color(0,255,255)
};
 
// postion of head to draw circle
PVector headPosition = new PVector();

// turn headPosition into scalar form
float distanceScalar;

// diameter of head drawn in pixels
float headSize = 200;
 
// threshold of level of confidence
float confidenceLevel = 0.5;

// the current confidence level that the kinect is tracking
float confidence;

// vector of tracked head for confidence checking
PVector confidenceVector = new PVector();

// vector to scalar ratio
float vectorScalar = 525;

// size of drawn dot on each jpint
float dotSize = 30;
 
// Vector values for all joints
PVector SKEL_HEAD = new PVector();
PVector SKEL_LEFT_SHOULDER = new PVector();
PVector SKEL_LEFT_ELBOW = new PVector();
PVector SKEL_LEFT_HAND = new PVector();
PVector SKEL_RIGHT_SHOULDER = new PVector();
PVector SKEL_RIGHT_ELBOW = new PVector();
PVector SKEL_RIGHT_HAND = new PVector();
PVector SKEL_TORSO = new PVector();
PVector SKEL_LEFT_HIP = new PVector();
PVector SKEL_LEFT_KNEE = new PVector();
PVector SKEL_LEFT_FOOT = new PVector();
PVector SKEL_RIGHT_HIP = new PVector();
PVector SKEL_RIGHT_KNEE = new PVector();
PVector SKEL_RIGHT_FOOT = new PVector();
 
// z coordinates of each limb
float SKEL_HEADZ;
float SKEL_LEFT_SHOULDERZ;
float SKEL_LEFT_ELBOWZ;
float SKEL_LEFT_HANDZ;
float SKEL_RIGHT_SHOULDERZ;
float SKEL_RIGHT_ELBOWZ;
float SKEL_RIGHT_HANDZ;
float SKEL_TORSOZ;
float SKEL_LEFT_HIPZ;
float SKEL_LEFT_KNEEZ;
float SKEL_LEFT_FOOTZ;
float SKEL_RIGHT_HIPZ;
float SKEL_RIGHT_KNEEZ;
float SKEL_RIGHT_FOOTZ;
 
// angle variables
float leftShoulderElbowX;
float leftShoulderElbowY;
float leftShoulderElbowZ;
float leftWristElbowX;
float leftWristElbowY;
float leftWristElbowZ;
 
float rightShoulderElbowX;
float rightShoulderElbowY;
float rightShoulderElbowZ;
float rightWristElbowX;
float rightWristElbowY;
float rightWristElbowZ;
 
float leftHipKneeX;
float leftHipKneeY;
float leftHipKneeZ;
float leftFootKneeX;
float leftFootKneeY;
float leftFootKneeZ;
 
float rightHipKneeX;
float rightHipKneeY;
float rightHipKneeZ;
float rightFootKneeX;
float rightFootKneeY;
float rightFootKneeZ;
 
// actual angles in radians of knees and elbows
float leftElbowAngle;
float rightElbowAngle;
float leftKneeAngle;
float rightKneeAngle;
 
/*---------------------------------------------------------------
Starts new kinect object and enables skeleton tracking.
Draws window
----------------------------------------------------------------*/
void setup(){

 // start a new kinect object
 kinect = new SimpleOpenNI(this);
 
 // enable depth sensor
 kinect.enableDepth();
 
 // enable skeleton generation for all joints
 kinect.enableUser();
 
 // draw thickness of drawer
 strokeWeight(3);
 // smooth out drawing
 smooth();
 
 // create a window the size of the depth information
 size(kinect.depthWidth(), kinect.depthHeight());
}
 
/*---------------------------------------------------------------
Updates Kinect. Gets users tracking and draws skeleton and
head if confidence of tracking is above threshold
----------------------------------------------------------------*/
void draw(){

	 // update the camera
	 kinect.update();

	 // get Kinect data
	 kinectDepth = kinect.depthImage();

	 // draw depth image at coordinates (0,0)
	 image(kinectDepth,0,0);
	 
	 // get all user IDs of tracked users
	 userID = kinect.getUsers();
	 
	 // loop through each user to see if tracking
	 for(int i=0;i confidenceLevel){

				 // change draw color based on hand id#
				 stroke(userColor[(i)]);
				 // fill the ellipse with the same color
				 fill(userColor[(i)]);
				 // get coordinates of all joints
				 getCoordinates(userID[i]);
				 // subtract vectors of limbs
				 subtractVectors();
				 // get angles of joints
				 getJointAngles();
			 }
	 	}
	 }
}
 
/*---------------------------------------------------------------
When a new user is found, print new user detected along with
userID and start pose detection. Input is userID
----------------------------------------------------------------*/
void onNewUser(SimpleOpenNI curContext, int userId){
 println("New User Detected - userId: " + userId);
 // start tracking of user id
 curContext.startTrackingSkeleton(userId);
} 
 
/*---------------------------------------------------------------
Print when user is lost. Input is int userId of user lost
----------------------------------------------------------------*/
void onLostUser(SimpleOpenNI curContext, int userId){
 // print user lost and user id
 println("User Lost - userId: " + userId);
} 
 
/*---------------------------------------------------------------
Called when a user is tracked.
----------------------------------------------------------------*/
void onVisibleUser(SimpleOpenNI curContext, int userId){
} 
 
/*---------------------------------------------------------------
Gets XYZ coordinates of all joints of tracked user and draws
a small circle on each joint
----------------------------------------------------------------*/
void getCoordinates(int userID){
 // get postion of all joints
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_HEAD,SKEL_HEAD);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_LEFT_SHOULDER,SKEL_LEFT_SHOULDER);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_LEFT_ELBOW,SKEL_LEFT_ELBOW);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_LEFT_HAND,SKEL_LEFT_HAND);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_RIGHT_SHOULDER,SKEL_RIGHT_SHOULDER);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_RIGHT_ELBOW,SKEL_RIGHT_ELBOW);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_RIGHT_HAND,SKEL_RIGHT_HAND);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_TORSO,SKEL_TORSO);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_LEFT_HIP,SKEL_LEFT_HIP);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_LEFT_KNEE,SKEL_LEFT_KNEE);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_LEFT_FOOT,SKEL_LEFT_FOOT);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_RIGHT_HIP,SKEL_RIGHT_HIP);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_RIGHT_KNEE,SKEL_RIGHT_KNEE);
 kinect.getJointPositionSkeleton(userID,
 SimpleOpenNI.SKEL_RIGHT_FOOT,SKEL_RIGHT_FOOT);
 
 // convert real world point to projective space
 kinect.convertRealWorldToProjective(SKEL_HEAD,SKEL_HEAD);
 kinect.convertRealWorldToProjective(SKEL_LEFT_SHOULDER,SKEL_LEFT_SHOULDER);
 kinect.convertRealWorldToProjective(SKEL_LEFT_ELBOW,SKEL_LEFT_ELBOW);
 kinect.convertRealWorldToProjective(SKEL_LEFT_HAND,SKEL_LEFT_HAND);
 kinect.convertRealWorldToProjective(SKEL_RIGHT_SHOULDER,SKEL_RIGHT_SHOULDER);
 kinect.convertRealWorldToProjective(SKEL_RIGHT_ELBOW,SKEL_RIGHT_ELBOW);
 kinect.convertRealWorldToProjective(SKEL_RIGHT_HAND,SKEL_RIGHT_HAND);
 kinect.convertRealWorldToProjective(SKEL_TORSO,SKEL_TORSO);
 kinect.convertRealWorldToProjective(SKEL_LEFT_HIP,SKEL_LEFT_HIP);
 kinect.convertRealWorldToProjective(SKEL_LEFT_KNEE,SKEL_LEFT_KNEE);
 kinect.convertRealWorldToProjective(SKEL_LEFT_FOOT,SKEL_LEFT_FOOT);
 kinect.convertRealWorldToProjective(SKEL_RIGHT_HIP,SKEL_RIGHT_HIP);
 kinect.convertRealWorldToProjective(SKEL_RIGHT_KNEE,SKEL_RIGHT_KNEE);
 kinect.convertRealWorldToProjective(SKEL_RIGHT_FOOT,SKEL_RIGHT_FOOT);
 
 // scale z vector of each joint to scalar form
 SKEL_HEADZ = (vectorScalar/SKEL_HEAD.z);
 SKEL_LEFT_SHOULDERZ = (vectorScalar/SKEL_LEFT_SHOULDER.z);
 SKEL_LEFT_ELBOWZ = (vectorScalar/SKEL_LEFT_ELBOW.z);
 SKEL_LEFT_HANDZ = (vectorScalar/SKEL_LEFT_HAND.z);
 SKEL_RIGHT_SHOULDERZ = (vectorScalar/SKEL_RIGHT_SHOULDER.z);
 SKEL_RIGHT_ELBOWZ = (vectorScalar/SKEL_RIGHT_ELBOW.z);
 SKEL_RIGHT_HANDZ = (vectorScalar/SKEL_RIGHT_HAND.z);
 SKEL_TORSOZ = (vectorScalar/SKEL_TORSO.z);
 SKEL_LEFT_HIPZ = (vectorScalar/SKEL_LEFT_HIP.z);
 SKEL_LEFT_KNEEZ = (vectorScalar/SKEL_LEFT_KNEE.z);
 SKEL_LEFT_FOOTZ = (vectorScalar/SKEL_LEFT_FOOT.z);
 SKEL_RIGHT_HIPZ = (vectorScalar/SKEL_RIGHT_HIP.z);
 SKEL_RIGHT_KNEEZ = (vectorScalar/SKEL_RIGHT_KNEE.z);
 SKEL_RIGHT_FOOTZ = (vectorScalar/SKEL_RIGHT_FOOT.z);
 
 // fill the dot color by the user id
 fill(userColor[userID-1]);
 
 // draw the circle at the position of the joint with the
 // diameter dependent on the z axis
 ellipse(SKEL_HEAD.x,SKEL_HEAD.y,SKEL_HEADZ*dotSize,SKEL_HEADZ*dotSize);
 ellipse(SKEL_LEFT_SHOULDER.x,SKEL_LEFT_SHOULDER.y,SKEL_LEFT_SHOULDERZ*dotSize,SKEL_LEFT_SHOULDERZ*dotSize);
 ellipse(SKEL_LEFT_ELBOW.x,SKEL_LEFT_ELBOW.y,SKEL_LEFT_ELBOWZ*dotSize,SKEL_LEFT_ELBOWZ*dotSize);
 ellipse(SKEL_LEFT_HAND.x,SKEL_LEFT_HAND.y,SKEL_LEFT_HANDZ*dotSize,SKEL_LEFT_HANDZ*dotSize);
 ellipse(SKEL_RIGHT_SHOULDER.x,SKEL_RIGHT_SHOULDER.y,SKEL_RIGHT_SHOULDERZ*dotSize,SKEL_RIGHT_SHOULDERZ*dotSize);
 ellipse(SKEL_RIGHT_ELBOW.x,SKEL_RIGHT_ELBOW.y,SKEL_RIGHT_ELBOWZ*dotSize,SKEL_RIGHT_ELBOWZ*dotSize);
 ellipse(SKEL_RIGHT_HAND.x,SKEL_RIGHT_HAND.y,SKEL_RIGHT_HANDZ*dotSize,SKEL_RIGHT_HANDZ*dotSize);
 ellipse(SKEL_TORSO.x,SKEL_TORSO.y,SKEL_TORSOZ*dotSize,SKEL_TORSOZ*dotSize);
 ellipse(SKEL_LEFT_HIP.x,SKEL_LEFT_HIP.y,SKEL_LEFT_HIPZ*dotSize,SKEL_LEFT_HIPZ*dotSize);
 ellipse(SKEL_LEFT_KNEE.x,SKEL_LEFT_KNEE.y,SKEL_LEFT_KNEEZ*dotSize,SKEL_LEFT_KNEEZ*dotSize);
 ellipse(SKEL_LEFT_FOOT.x,SKEL_LEFT_FOOT.y,SKEL_LEFT_FOOTZ*dotSize,SKEL_LEFT_FOOTZ*dotSize);
 ellipse(SKEL_RIGHT_HIP.x,SKEL_RIGHT_HIP.y,SKEL_RIGHT_HIPZ*dotSize,SKEL_RIGHT_HIPZ*dotSize);
 ellipse(SKEL_RIGHT_KNEE.x,SKEL_RIGHT_KNEE.y,SKEL_RIGHT_KNEEZ*dotSize,SKEL_RIGHT_KNEEZ*dotSize);
 ellipse(SKEL_RIGHT_FOOT.x,SKEL_RIGHT_FOOT.y,SKEL_RIGHT_FOOTZ*dotSize,SKEL_RIGHT_FOOTZ*dotSize);
}

/*---------------------------------------------------------------
Subtracts each vector from each limb combination from each other
----------------------------------------------------------------*/
void subtractVectors() {
 // take vector[] shoulder and subtract from vector[] elbow
 leftShoulderElbowX = SKEL_LEFT_SHOULDER.x - SKEL_LEFT_ELBOW.x;
 leftShoulderElbowY = SKEL_LEFT_SHOULDER.y - SKEL_LEFT_ELBOW.y;
 leftShoulderElbowZ = SKEL_LEFT_SHOULDER.z - SKEL_LEFT_ELBOW.z;

 // take vector[] hand and subtract from vector[] elbow
 leftWristElbowX = SKEL_LEFT_HAND.x - SKEL_LEFT_ELBOW.x;
 leftWristElbowY = SKEL_LEFT_HAND.y - SKEL_LEFT_ELBOW.y;
 leftWristElbowZ = SKEL_LEFT_HAND.z - SKEL_LEFT_ELBOW.z;

 // take vector[] shoulder and subtract from vector[] elbow
 rightShoulderElbowX = SKEL_RIGHT_SHOULDER.x - SKEL_RIGHT_ELBOW.x;
 rightShoulderElbowY = SKEL_RIGHT_SHOULDER.y - SKEL_RIGHT_ELBOW.y;
 rightShoulderElbowZ = SKEL_RIGHT_SHOULDER.z - SKEL_RIGHT_ELBOW.z;

 // take vector[] hand and subtract from vector[] elbow
 rightWristElbowX = SKEL_RIGHT_HAND.x - SKEL_RIGHT_ELBOW.x;
 rightWristElbowY = SKEL_RIGHT_HAND.y - SKEL_RIGHT_ELBOW.y;
 rightWristElbowZ = SKEL_RIGHT_HAND.z - SKEL_RIGHT_ELBOW.z;
 
 // take vector[] hip and subtract from vector[] knee
 leftHipKneeX = SKEL_LEFT_HIP.x - SKEL_LEFT_KNEE.x;
 leftHipKneeY = SKEL_LEFT_HIP.y - SKEL_LEFT_KNEE.y;
 leftHipKneeZ = SKEL_LEFT_HIP.z - SKEL_LEFT_KNEE.z;

 // take vector[] foot and subtract from vector[] knee
 leftFootKneeX = SKEL_LEFT_FOOT.x - SKEL_LEFT_KNEE.x;
 leftFootKneeY = SKEL_LEFT_FOOT.y - SKEL_LEFT_KNEE.y;
 leftFootKneeZ = SKEL_LEFT_FOOT.z - SKEL_LEFT_KNEE.z;

 // take vector[] hip and subtract from vector[] knee
 rightHipKneeX = SKEL_RIGHT_HIP.x - SKEL_RIGHT_KNEE.x;
 rightHipKneeY = SKEL_RIGHT_HIP.y - SKEL_RIGHT_KNEE.y;
 rightHipKneeZ = SKEL_RIGHT_HIP.z - SKEL_RIGHT_KNEE.z;

 // take vector[] foot and subtract from vector[] knee
 rightFootKneeX = SKEL_RIGHT_FOOT.x - SKEL_RIGHT_KNEE.x;
 rightFootKneeY = SKEL_RIGHT_FOOT.y - SKEL_RIGHT_KNEE.y;
 rightFootKneeZ = SKEL_RIGHT_FOOT.z - SKEL_RIGHT_KNEE.z;
}
 
/*---------------------------------------------------------------
Gets angles of joints based on Z axis and prints them.
Example math:
 
Let the coordinates of elbow (x,y,z) be denoted as E,
shoulder S, and wrist W.
 
Define the vector EW = W - E = (x_w-x_e, y_w-y_e, z_w,z_e)
and ES = S - E.
 
Then the angle between vector EW and vector ES is given
by arccos(EW dot ES / (mag(EW) * mag (ES)).
 
Where mag(X) is the magnitude of a vector, given by
sqrt(x2 + y2 + z2 ), and
 
A dot B is the dot product of
vectors A and B, given by A_x*B_x + A_y*B_y + A_z*B_z
----------------------------------------------------------------*/
void getJointAngles() {
 leftElbowAngle = acos((leftShoulderElbowX*leftWristElbowX+leftShoulderElbowY*leftWristElbowY+leftShoulderElbowZ*leftWristElbowZ)/(sqrt(leftWristElbowX*leftWristElbowX+leftWristElbowY*leftWristElbowY+leftWristElbowZ*leftWristElbowZ)*(sqrt(leftShoulderElbowX*leftShoulderElbowX+leftShoulderElbowY*leftShoulderElbowY+leftShoulderElbowZ*leftShoulderElbowZ))));

 rightElbowAngle = acos((rightShoulderElbowX*rightWristElbowX+rightShoulderElbowY*rightWristElbowY+rightShoulderElbowZ*rightWristElbowZ)/(sqrt(rightWristElbowX*rightWristElbowX+rightWristElbowY*rightWristElbowY+rightWristElbowZ*rightWristElbowZ)*(sqrt(rightShoulderElbowX*rightShoulderElbowX+rightShoulderElbowY*rightShoulderElbowY+rightShoulderElbowZ*rightShoulderElbowZ))));
 
 leftKneeAngle = acos((leftHipKneeX*leftFootKneeX+leftHipKneeY*leftFootKneeY+leftHipKneeZ*leftFootKneeZ)/(sqrt(leftFootKneeX*leftFootKneeX+leftFootKneeY*leftFootKneeY+leftFootKneeZ*leftFootKneeZ)*(sqrt(leftHipKneeX*leftHipKneeX+leftHipKneeY*leftHipKneeY+leftHipKneeZ*leftHipKneeZ))));
 
 rightKneeAngle = acos((rightHipKneeX*rightFootKneeX+rightHipKneeY*rightFootKneeY+rightHipKneeZ*rightFootKneeZ)/(sqrt(rightFootKneeX*rightFootKneeX+rightFootKneeY*rightFootKneeY+rightFootKneeZ*rightFootKneeZ)*(sqrt(rightHipKneeX*rightHipKneeX+rightHipKneeY*rightHipKneeY+rightHipKneeZ*rightHipKneeZ))));
 
 // print angles
 print("Left elbow angle: ");
 println(leftElbowAngle);
 print("Right elbow angle: ");
 println(rightElbowAngle);
 print("Left knee angle: ");
 println(leftKneeAngle);
 print("Right knee angle: ");
 println(rightKneeAngle);
}
	

Leave a Reply

Your email address will not be published.