Kinect skeleton Scaling strange behaviour - scale

Question

I am trying to scale a skeleton to match to the sizes of another skeleton.
My algoritm do the following:
Find the distance between two joints of the origin skeleton and the destiny skeleton using phytagorean teorem
divide this two distances to find a multiply factor.
Multiply each joint by this factor.
Here is my actual code:
public static Skeleton ScaleToMatch(this Skeleton skToBeScaled, Skeleton skDestiny)
{
Joint newJoint = new Joint();
double distanciaOrigem = 0;
double distanciaDestino = 0;
double fator = 1;
SkeletonPoint pos = new SkeletonPoint();
foreach (BoneOrientation bo in skToBeScaled.BoneOrientations)
{
distanciaOrigem = FisioKinectCalcs.Distance3DBetweenJoint(skToBeScaled.Joints[bo.StartJoint], skToBeScaled.Joints[bo.EndJoint]);
distanciaDestino = FisioKinectCalcs.Distance3DBetweenJoint(skDestiny.Joints[bo.StartJoint], skDestiny.Joints[bo.EndJoint]);
if (distanciaOrigem > 0 && distanciaDestino > 0)
{
fator = (distanciaDestino / distanciaOrigem);
newJoint = skToBeScaled.Joints[bo.EndJoint]; // escaling only the end joint as the BoneOrientatios starts from HipCenter, i am scaling from center to edges.
// applying the new values to the joint
pos = new SkeletonPoint()
{
X = (float)(newJoint.Position.X * fator),
Y = (float)(newJoint.Position.Y * fator),
Z = (float)(newJoint.Position.Z * fator)
};
newJoint.Position = pos;
skToBeScaled.Joints[bo.EndJoint] = newJoint;
}
}
return skToBeScaled;
}
Every seems to work fine except for the hands and foots
Look at this images
I have my own skeleton over me, and my skeleton scaled to the sizes of another person, but the hands and foots still crazy. (but code looks right)
Any suggestion?

Answer 1

It's hard to say without running the code, but it somewhat "looks good".
What I would validate though, is your
if (distanciaOrigem > 0 && distanciaDestino > 0)
If distanciaOrigem is very close to 0, but even just epsilon away from 0, it won't be picked up by the if, and then
fator = (distanciaDestino / distanciaOrigem);
Will result in a very large number!

Answer 2

I would suggest to smooth the factor so it generally fits the proper scale. Try this code:
private static Dictionary<JointType, double> jointFactors = null;
static CalibrationUtils()
{
InitJointFactors();
}
public static class EnumUtil
{
public static IEnumerable<T> GetValues<T>()
{
return Enum.GetValues(typeof(T)).Cast<T>();
}
}
private static void InitJointFactors()
{
var jointTypes = EnumUtil.GetValues<JointType>();
jointFactors = new Dictionary<JointType, double>();
foreach(JointType type in jointTypes)
{
jointFactors.Add(type, 0);
}
}
private static double SmoothenFactor(JointType jointType, double factor, int weight)
{
double currentValue = jointFactors[jointType];
double newValue = 0;
if(currentValue != 0)
newValue = (weight * currentValue + factor) / (weight + 1);
else
newValue = factor;
jointFactors[jointType] = newValue;
return newValue;
}
When it comes to factor usage just use the SmoothenFactor method first:
public static Skeleton ScaleToMatch(this Skeleton skToBeScaled, Skeleton skDestiny, double additionalFactor = 1)
{
Joint newJoint = new Joint();
double distanceToScale = 0;
double distanceDestiny = 0;
double factor = 1;
int weight = 500;
SkeletonPoint pos = new SkeletonPoint();
Skeleton newSkeleton = null;
KinectHelper.CopySkeleton(skToBeScaled, ref newSkeleton);
SkeletonPoint hipCenterPosition = newSkeleton.Joints[JointType.HipCenter].Position;
foreach(BoneOrientation bo in skToBeScaled.BoneOrientations)
{
distanceToScale = Distance3DBetweenJoints(skToBeScaled.Joints[bo.StartJoint], skToBeScaled.Joints[bo.EndJoint]);
distanceDestiny = Distance3DBetweenJoints(skDestiny.Joints[bo.StartJoint], skDestiny.Joints[bo.EndJoint]);
if(distanceToScale > 0 && distanceDestiny > 0)
{
factor = (distanceDestiny / distanceToScale) * additionalFactor;
newJoint = skToBeScaled.Joints[bo.EndJoint]; // escaling only the end joint as the BoneOrientatios starts from HipCenter, i am scaling from center to edges.
factor = SmoothenFactor(newJoint.JointType, factor, weight);
pos = new SkeletonPoint()
{
X = (float)((newJoint.Position.X - hipCenterPosition.X) * factor + hipCenterPosition.X),
Y = (float)((newJoint.Position.Y - hipCenterPosition.Y) * factor + hipCenterPosition.Y),
Z = (float)((newJoint.Position.Z - hipCenterPosition.Z) * factor + hipCenterPosition.Z)
};
newJoint.Position = pos;
newSkeleton.Joints[bo.EndJoint] = newJoint;
}
}
return newSkeleton;
}
I also modified your ScaleToMatch method as you see. There was a need to move joints in relation to HipCenter position. Also new positions are saved to a new Skeleton instance so they are not used in further vector calculations.
Experiment with the weight but since our bones length is constant you can use big numbers like 100 and more to be sure that wrong Kinect readings do not disturb the correct scale.
Here's an example of how it helped with scaling HandRight joint position:
The weight was set to 500. The resulting factor is supposed to be around 2 (because the base skeleton was purposely downscaled by a factor of 2).
I hope it helps!