///<summary>
/// At the beginnig a calibration is made by making an average of 5 frame
///</summary>
public void calibrate(Skeleton[] skeletons)
{
Skeleton skel = (from trackskeleton in skeletons
where trackskeleton.TrackingState == SkeletonTrackingState.Tracked
select trackskeleton).FirstOrDefault();
if (skel != null)
{
if (this.iterationCalbration == 0)
{
foreach (Joint joint in skel.Joints)
{
//Calibration is made getting the first position of the skeleton
//the position of the user should be almost equal to the init pose of the NAO
this.calibrationValue.Add(joint.JointType, JointUtilities.averagePoint(joint.Position, this.avgCalibration));
}
this.iterationCalbration++;
}
else if (this.iterationCalbration > 0 && this.iterationCalbration < this.avgCalibration)
{
foreach (Joint joint in skel.Joints)
{
this.calibrationValue[joint.JointType] = JointUtilities.averagePoint(this.calibrationValue[joint.JointType],
joint.Position,
this.avgCalibration);
}
this.iterationCalbration++;
}
else
{
this.endCalibration();
}
}
}
///<summary>
///
///</summary>
public void getPoint(Skeleton[] skeletons)
{
Skeleton skel = (from trackskeleton in skeletons
where trackskeleton.TrackingState == SkeletonTrackingState.Tracked
select trackskeleton).FirstOrDefault();
if (skel != null)
{
if (this.iteration == 0)
{
//fill the dictionary with the new point
foreach (Joint joint in skel.Joints)
{
this.avgPoint.Add(joint.JointType, JointUtilities.averagePoint(joint.Position, this.avgCalibration));
}
this.angleMatrix(skel);
this.iteration++;
}
else if (this.iteration > 0 && this.iteration < this.avg)
{
//Continue to fill the dictionary making an average with the old inserted point
foreach (Joint joint in skel.Joints)
{
this.avgPoint[joint.JointType] = JointUtilities.averagePoint(this.avgPoint[joint.JointType],
joint.Position,
this.avgCalibration);
}
this.angleMatrix(skel);
this.iteration++;
}
else
{
//call the function to compute the angle
this.angleVector();
//call the function to move the joint
this.moveJoint();
//reset iteration
this.iteration = 0;
//remove the old value from the dictionary
this.avgPoint.Clear();
}
}
}
