Calculating Center of mass of body being tracked using kinect?

Question

I am working on Kinect for my research project . I have worked previously to calculate the joint angle of kinect and the joint coordinates. I would like to calculate the center of mass of the body which is being tracked. Any idea would be appreciated and code snippets would be immensely helpful. I owe a lot to stack overflow without the community help it would had not been possible to do such a thing. Thanks in Advance Please find the code where i want to include this center of mass function. This function tracks the skeleton.

Skeleton GetFirstSkeleton(AllFramesReadyEventArgs e) { using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame()) { if (skeletonFrameData == null) { return null; }

            skeletonFrameData.CopySkeletonDataTo(allSkeletons);

            //get the first tracked skeleton
            Skeleton first = (from s in allSkeletons
                              where s.TrackingState == SkeletonTrackingState.Tracked
                              select s).FirstOrDefault();
            return first;
        }

I have tried using this code in my code but its not getting accustomed , can any one please help me include the center of mass code.

oreach (SkeletonData data in skeletonFrame.Skeletons) {

SkeletonFrame allskeleton = e.SkeletonFrame;

            // Count passive and active person up to six in the group  

            int numberOfSkeletonsT = (from s in allskeleton.Skeletons

                                      where s.TrackingState == SkeletonTrackingState.Tracked  select s).Count();

            int numberOfSkeletonsP = (from s in allskeleton.Skeletons  

where s.TrackingState == SkeletonTrackingState.PositionOnly select s).Count();

            // Count passive and active person up to six in the group

            int totalSkeletons = numberOfSkeletonsP + numberOfSkeletonsT;

            //Console.WriteLine("TotalSkeletons = " + totalSkeletons);

//======================================================

if (data.TrackingState == SkeletonTrackingState.PositionOnly)

            {

foreach (Joint joint in data.Joints)

                {

                    if (joint.Position.Z != 0)

                    {

double centerofmassX = com.Position.X;

                        double centerofmassY = com.Position.Y;

                        double centerofmassZ = com.Position.Z;

   Console.WriteLine( centerofmassX +  centerofmassY + centerofmassZ );

                    }

            }

Answer 1

See a couple of resources here:

1. http://mathwiki.ucdavis.edu/Calculus/Vector_Calculus/Multiple_Integrals/Moments_and_Centers_of_Mass#Three-Dimensional_Solids
2. http://www.slideshare.net/GillianWinters/center-of-mass-presentation
3. http://en.wikipedia.org/wiki/Locating_the_center_of_mass

Basically no matter what, you are going to need to find the mass of your user. This can be a simple input, then you can determine how much weight the person puts on each foot and use the equations described at all of these sources. Another option may be to use plumb lines on a planar shape representation of the user in 2D, However that won't be the actually accurate 3D center of mass.

Here is an example of how to find what amount of mass is on each foot. using the equation found on http://www.vitutor.com/geometry/distance/line_plane.html

Vector3 v = new Vector3(skeleton.Joints[JointType.Head].Position.X,    skeleton.Joints[JointType.Head].Position.Y, skeleton.Joints[JointType.Head].Position.Z);
double mass;
double leftM, rightM;

double A = sFrame.FloorClipPlane.X,
B = sFrame.FloorClipPlane.Y,
C = sFrame.FloorClipPlane.Z;

//find angle
double angle = Math.ASin(Math.Abs(A * v.X + B * v.Y * C * v.Z)/(Math.Sqrt(A * A + B * B + C * C) * Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z)));

if (angle == 90.0)
{
     leftM = mass / 2.0;
     rightM = mass / 2.0;
}

double distanceFrom90 = 90.0 - angle;

if (distanceFrom90 > 0)
{
    double leftMultiple = distanceFrom90 / 90.0;
    leftM = mass * leftMultiple;
    rightM = mass - leftM;
}

else
{
    double rightMultiple = distanceFrom90 / 90.0;
    rightM = rightMultiple * mass;
    leftM = mass - rightMultiple;
}

This is of course assuming that the user is on both feet, but you could modify the code to create a new plane based off the users feet instead of the automatic one generated by Kinect.

The code to then find the center of mass you have to choose a datum. I would choose the head as that is the top of the person, and you can measure down from it easily. Using the steps found here:

double distanceFromDatumLeft = Math.Sqrt(Math.Pow(headX - footLeftX, 2) + Math.Pow(headY - footLeftY, 2) + Math.Pow(headZ - footLeftZ, 2));
double distanceFromDatumLeft = Math.Sqrt(Math.Pow(headX - footRightX, 2) + Math.Pow(headY - footRightY, 2) + Math.Pow(headZ - footRightZ, 2));

double momentLeft = distanceFromDatumLeft * leftM;
double momentRight = distanceFromDatumRight * rightM;

double momentSum = momentLeft + momentRight;

//measured in units from the datum
double centerOfGravity = momentSum / mass;

You then can of course show this on the screen by passing a point to plot that is centerOfGravity points below the head.