} // 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; } }