} // EndMethod
public static SkeletonOfBody GetSkeletonUpperBody(CoordinateMapper mapper, Body body)
{
// Skeleton structure
SkeletonOfBody skel = new SkeletonOfBody(Constants.SKEL_UP_TOTAL_JOINTS);
// Process the tracked body
if (body != null && body.IsTracked)
{
//Extract body information
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
IReadOnlyDictionary <JointType, JointOrientation> orientations = body.JointOrientations;
int cont = 0;
foreach (var pos in Constants.SKEL_UP_INDEX)
{
Point pointInColor = MathFunc.ToPoint(mapper, VisTypes.Color, joints[(JointType)pos].Position);
Point pointInDepth = MathFunc.ToPoint(mapper, VisTypes.Depth, joints[(JointType)pos].Position);
// Discard joint that are not being tracked
if (joints[(JointType)pos].TrackingState == TrackingState.Tracked || joints[(JointType)pos].TrackingState == TrackingState.Inferred)
{
skel.jointName[cont] = joints[(JointType)pos].JointType.ToString().Trim(); // Name
skel.jointColorSpace[cont] = pointInColor; // 2D Point
skel.jointDepthSpace[cont] = pointInDepth; // 2D Point
skel.jointCameraSpace[cont] = joints[(JointType)pos].Position; // 3D Point
skel.jointQuaternion[cont] = orientations[(JointType)pos].Orientation; // Vector 4D
}
cont++;
} // EndForEach
} // EndIf
return(skel);
} // EndMethod
} //End Method
public static void SaveSkeletonUpFrames(this DepthSensorKinect2 sensor, string _filenamePattern)
{
List <IList <Body> > _listOfBodies = sensor.Data.AllListOfBodies;
int fileCount = 1;
string filename;
SkeletonOfBody skeleton = new SkeletonOfBody(Constants.SKEL_UP_TOTAL_JOINTS);
// Traverse the skeleton list
foreach (IList <Body> _bodies in _listOfBodies)
{
// Get the most closest tracked skeleton
Body body = Util.GetClosestBody(_bodies);
//Body body = _bodies.Where(b => b.IsTracked).FirstOrDefault();
// Process the tracked body
if (body != null && body.IsTracked)
{
// Get the upper skeleton
skeleton = Util.GetSkeletonUpperBody(sensor.Mapper, body);
} // EndIf
// Write the skeleton position in a file
filename = _filenamePattern + MathFunc.ToFourDigits(fileCount) + ".csv";
Util.Skeleton2File(filename, skeleton);
fileCount++;
}// EndForEach
Console.WriteLine("Saved {0} Body Data Files", fileCount - 1);
} //End Method
}// EndMethod
public static void Skeleton2File(string _filename, SkeletonOfBody skeleton)
{
try
{
using (StreamWriter sw = new StreamWriter(_filename))
{
for (int i = 0; i < skeleton.jointName.Count(); i++)
{
sw.WriteLine("{0}; {1}; {2}; {3}; {4}",
skeleton.jointName[i],
skeleton.jointColorSpace[i].X,
skeleton.jointColorSpace[i].Y,
0,
0
);
sw.WriteLine("{0}; {1}; {2}; {3}; {4}",
skeleton.jointName[i],
skeleton.jointDepthSpace[i].X,
skeleton.jointDepthSpace[i].Y,
0,
0
);
sw.WriteLine("{0}; {1}; {2}; {3}; {4}",
skeleton.jointName[i],
skeleton.jointCameraSpace[i].X,
skeleton.jointCameraSpace[i].Y,
skeleton.jointCameraSpace[i].Z,
0
);
sw.WriteLine("{0}; {1}; {2}; {3}; {4}",
skeleton.jointName[i],
skeleton.jointQuaternion[i].X,
skeleton.jointQuaternion[i].Y,
skeleton.jointQuaternion[i].Z,
skeleton.jointQuaternion[i].W
);
}
}
}
catch (IOException)
{
Console.WriteLine("Cannot write {0}", _filename);
}
}
} //EndMethod
public static void SaveBodyShotUpperJoints(IList <Body> _listBodies, CoordinateMapper mapper, string _path, string _source)
{
SkeletonOfBody skeleton = new SkeletonOfBody(Constants.SKEL_UP_TOTAL_JOINTS);
string filename = System.IO.Path.Combine(_path + "-" + _source + ".csv");
Body body = Util.GetClosestBody(_listBodies);
// Process the tracked body
if (body != null && body.IsTracked)
{
// Get the complete skeleton
skeleton = Util.GetSkeletonUpperBody(mapper, body);
} // EndIf
// Write the skeleton position in a file
Util.Skeleton2File(filename, skeleton);
} //EndMethod
public static void WriteSkeletonOverFrame(DepthSensorKinect2 sensor, VisTypes visType, SkeletonOfBody skeleton, int sizePoint, ref ushort[] buffDepth16)
{
// Draw joints pixels over the depth image
switch (visType)
{
case VisTypes.Color:
// Overwrite the points over the depth image in color space
foreach (var pos in skeleton.jointColorSpace)
{
for (int m = -sizePoint; m <= sizePoint; m++)
{
for (int n = -sizePoint; n <= sizePoint; n++)
{
int posX = (int)pos.X + m;
int posY = (int)pos.Y + n;
if ((posX >= 0 && posX < sensor.ColorImageSize.Width) && (posY >= 0 && posY < sensor.ColorImageSize.Height))
{
int indexDepth = posY * sensor.ColorImageSize.Width + posX;
// Highlight the pixel
buffDepth16[indexDepth] = 0xffff;
}
}
}
}
break;
case VisTypes.Depth:
case VisTypes.Infrared:
case VisTypes.BodyIndex:
// Overwrite the points over the depth image in depth space
foreach (var pos in skeleton.jointDepthSpace)
{
for (int m = -sizePoint; m <= sizePoint; m++)
{
for (int n = -sizePoint; n <= sizePoint; n++)
{
int posX = (int)pos.X + m;
int posY = (int)pos.Y + n;
if ((posX >= 0 && posX < sensor.DepthImageSize.Width) && (posY >= 0 && posY < sensor.DepthImageSize.Height))
{
int indexDepth = posY * sensor.DepthImageSize.Width + posX;
// Highlight the pixel
buffDepth16[indexDepth] = 0xffff;
}
}
}
}
break;
case VisTypes.None:
break;
default:
break;
}
}
