none
Adding skeleton joint function RRS feed

  • General discussion

  • Hello everyone,

    I have a code for recording the RGB, Depth, and IR video streams. I want to add one more function to save the  skeleton joints in .txt file.
    Can you help me to point out where and how can that function in that code?
    **********************************************code************************************
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;
    using System.Windows;
    using System.Windows.Controls;
    using System.Windows.Data;
    using System.Windows.Documents;
    using System.Windows.Input;
    using System.Windows.Media;
    using System.Windows.Media.Imaging;
    using System.Windows.Navigation;
    using System.Windows.Shapes;
    using System.Threading;
    
    using System.IO;
    using System.Drawing;
    
    using Microsoft.Kinect;
    using OpenCvSharp;
    using OpenCvSharp.Utilities;
    
    namespace Kinect2_Wpf
    {
        public partial class MainWindow : Window
        {
            KinectSensor sensor;
    
            WriteableBitmap WbDepth;
            WriteableBitmap WbColor;
            WriteableBitmap WbIR;
            WriteableBitmap WbLongIR;
            WriteableBitmap WbDepthAddCol;
    
            int DWidth, DHeight;
            int CWidth, CHeight;
            int IRWidth, IRHeight;
    
            ColorSpacePoint[] colorspacePoint;
            DepthSpacePoint[] depthspacePoint;
    
            byte[] Colordata;
            ushort[] Depthdata;
            ushort[] IRdata;
            ushort[] LongExpIRdata;
            byte[] DepthdataAddCol;
            
            IList<Body> _bodies;
            bool _trackingBody;
            int _bodyFrameCount = 0;
            
            int strideCol;
            int strideDep;
            int strideLIR;
            int strideIR ;
            int strideDepAddCol;
            
            int scene = 0;
            bool Record = false;
    
            IplImage color = new IplImage(1920, 1080, BitDepth.U8, 4);
            IplImage depth = new IplImage(512, 424, BitDepth.U16, 1);
            IplImage IR = new IplImage(512, 424, BitDepth.U16, 1);
            IplImage CropColor = new IplImage(512, 424, BitDepth.U8, 4);
            IplImage bodyColor = new IplImage(512, 424, BitDepth.U8, 4);
    
            CvVideoWriter ImageWriter;// = new CvVideoWriter("ImageRecord.avi", FourCC.Default, 30.0f, new CvSize(1920, 1080));
            CvVideoWriter DepthWriter;// = new CvVideoWriter("DepthRecord.avi", FourCC.Default, 30.0f, new CvSize(512, 424));
            CvVideoWriter IRWriter;// = new CvVideoWriter("DepthRecord.avi", FourCC.Default, 30.0f, new CvSize(512, 424));
            CvVideoWriter ImgCropWriter;// = new CvVideoWriter("DepthRecord.avi", FourCC.Default, 30.0f, new CvSize(512, 424));
            CvVideoWriter BodyWriter;
    
            public static string data = null;
    
            public MainWindow()
            {
                InitializeComponent();
            }
    
            private void Window_Loaded(object sender, RoutedEventArgs e)
            {
                sensor = KinectSensor.GetDefault();
                sensor.Open();
    
                DWidth  = sensor.DepthFrameSource.FrameDescription.Width;
                DHeight = sensor.DepthFrameSource.FrameDescription.Height;
                CWidth = sensor.ColorFrameSource.FrameDescription.Width;
                CHeight = sensor.ColorFrameSource.FrameDescription.Height;
                IRWidth = sensor.LongExposureInfraredFrameSource.FrameDescription.Width;
                IRHeight = sensor.LongExposureInfraredFrameSource.FrameDescription.Height;
    
                WbColor = new WriteableBitmap(CWidth, CHeight, 96, 96, PixelFormats.Bgra32, null);
                WbDepth = new WriteableBitmap(DWidth, DHeight, 96, 96, PixelFormats.Gray16, null);
                WbIR = new WriteableBitmap(IRWidth, IRHeight, 96, 96, PixelFormats.Gray16, null);
                WbLongIR = new WriteableBitmap(IRWidth, IRHeight, 96, 96, PixelFormats.Gray16, null);
                WbDepthAddCol = new WriteableBitmap(DWidth, DHeight, 96, 96, PixelFormats.Bgra32, null);
        
                strideCol = CWidth * 4;// (int)frame.ColorFrameSource.FrameDescription.BytesPerPixel;
                strideDep = DWidth * (int)sensor.DepthFrameSource.FrameDescription.BytesPerPixel;
    
                strideLIR = IRWidth * (int)sensor.LongExposureInfraredFrameSource.FrameDescription.BytesPerPixel;
                strideIR = IRWidth * (int)sensor.InfraredFrameSource.FrameDescription.BytesPerPixel;
                strideDepAddCol = DWidth * 4;
    
                MultiSourceFrameReader Multireader = sensor.OpenMultiSourceFrameReader(FrameSourceTypes.Color | FrameSourceTypes.Depth |FrameSourceTypes.LongExposureInfrared | FrameSourceTypes.Infrared | FrameSourceTypes.Body | FrameSourceTypes.BodyIndex);
                Multireader.MultiSourceFrameArrived +=Multireader_MultiSourceFrameArrived;
            }
    
            void Multireader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
            {
                int depthshft = (int)SliderDepth.Value;
                int IRshft = (int)SliderIR.Value;
                MultiSourceFrame frame = e.FrameReference.AcquireFrame();
                using (LongExposureInfraredFrame LongExIRframe = frame.LongExposureInfraredFrameReference.AcquireFrame())
                using (InfraredFrame IRframe = frame.InfraredFrameReference.AcquireFrame())
                using (ColorFrame colorframe = frame.ColorFrameReference.AcquireFrame())
                using (DepthFrame depthframe = frame.DepthFrameReference.AcquireFrame())
                using (BodyFrame bodyframe = frame.BodyFrameReference.AcquireFrame())
                using (BodyIndexFrame bodyindexframe = frame.BodyIndexFrameReference.AcquireFrame())
                {
                    try
                    {
                        unsafe
                        {
                            ushort* depthdata = (ushort*)depth.ImageData;
                            byte* imagedata = (byte*)color.ImageDataPtr;
                        
                            if (colorframe != null)
                            {
                                Colordata = new byte[CWidth * CHeight * 4];
                                colorframe.CopyConvertedFrameDataToArray(Colordata, ColorImageFormat.Bgra);
                                color.CopyPixelData(Colordata);
                                
                            }
                            
                            if (depthframe != null)
                            {
                                Depthdata = new ushort[DWidth * DHeight];
                                ushort[] Depthloc = new ushort[DWidth * DHeight];
                                DepthdataAddCol = new byte[DWidth * DHeight * 4];
                                depthframe.CopyFrameDataToArray(Depthdata);
    
                                for (int i = 0; i < DWidth * DHeight;i++)
                                {
                                    Depthloc[i] = 0x1000;
                                }
                                
                                colorspacePoint = new ColorSpacePoint[DWidth * DHeight];
                                depthspacePoint = new DepthSpacePoint[CWidth * CHeight];
                                sensor.CoordinateMapper.MapDepthFrameToColorSpace(Depthloc, colorspacePoint);
                                
                                for (int y = 0; y < DHeight; y++)
                                {
                                    for (int x = 0; x < DWidth; x++)
                                    {
                                        if (depthshft != 0)
                                        {
                                            Depthdata[y * DWidth + x] = (ushort)((Depthdata[y * DWidth + x]) << depthshft);
                                        }
    
                                        int colorX = (int)(colorspacePoint[y * DWidth + x].X);
                                        int colorY = (int)(colorspacePoint[y * DWidth + x].Y);
                                        if ((colorX >= 0) && (colorX < CWidth) && (colorY >= 0) && (colorY < CHeight))
                                        {
                                            DepthdataAddCol[y * 4 * DWidth + 4 * x + 0] = (Colordata[colorY * 4 * CWidth + 4 * colorX + 0]); //B
                                            DepthdataAddCol[y * 4 * DWidth + 4 * x + 1] = (Colordata[colorY * 4 * CWidth + 4 * colorX + 1]); //G
                                            DepthdataAddCol[y * 4 * DWidth + 4 * x + 2] = (Colordata[colorY * 4 * CWidth + 4 * colorX + 2]); //R
                                            DepthdataAddCol[y * 4 * DWidth + 4 * x + 3] = (Colordata[colorY * 4 * CWidth + 4 * colorX + 3]); //alpha
                                        }
                                    }
                                }
                                depth.CopyPixelData(Depthdata);
                                CropColor.CopyPixelData(DepthdataAddCol);
                                 
                            }
                            
                            if (IRframe != null)
                            {
                                IRdata = new ushort[IRWidth * IRHeight];
                                IRframe.CopyFrameDataToArray(IRdata);
                                if (IRshft != 0)
                                {
                                    for (int y = 0; y < DHeight; y++)
                                    {
                                        for (int x = 0; x < DWidth; x++)
                                        {
                                            long temp = (long)((IRdata[y * DWidth + x]) << IRshft);
                                            if (temp > ushort.MaxValue) temp = ushort.MaxValue;
                                            IRdata[y * DWidth + x] = (ushort)(temp);
                                        }
                                    }
                                }
                                IR.CopyPixelData(IRdata);
                             
                            }
    
                            if (bodyframe != null)
                            {
                                _bodies = new Body[bodyframe.BodyFrameSource.BodyCount];
                                bodyframe.GetAndRefreshBodyData(_bodies);
                                
                                bodyColor.Zero();
                                int cntbodies = 0;
                                foreach (var body in _bodies)
                                {
                                    if (body != null)
                                    {
                                        if (body.IsTracked)
                                        {
                                            CvScalar col = new CvScalar();
                                            switch(cntbodies)
                                            {
                                                case 0 :
                                                    col = new CvScalar(0, 0, 255);
                                                    break;
                                                case 1:
                                                    col = new CvScalar(0, 255, 0);
                                                    break;
                                                case 2:
                                                    col = new CvScalar(255, 0, 0);
                                                    break;
                                                case 3:
                                                    col = new CvScalar(0, 255, 255);
                                                    break;
                                                case 4:
                                                    col = new CvScalar(255, 0, 255);
                                                    break;
                                                case 5:
                                                    col = new CvScalar(255, 255, 0);
                                                    break;
                                            }
                                            CameraSpacePoint[] cameraSpacePoint = new CameraSpacePoint[25];
                                            DepthSpacePoint[] depthSpacePoint = new DepthSpacePoint[25];
                                            
                                            IList<Joint> joints = new Joint[25];
                                            
                                            joints[0] = body.Joints[JointType.HandTipLeft];
                                            joints[1] = body.Joints[JointType.HandLeft];
                                            joints[2] = body.Joints[JointType.WristLeft];
                                            joints[3] = body.Joints[JointType.ElbowLeft];
                                            joints[4] = body.Joints[JointType.ShoulderLeft];
    
                                            joints[5] = body.Joints[JointType.HandTipRight];
                                            joints[6] = body.Joints[JointType.HandRight];
                                            joints[7] = body.Joints[JointType.WristRight];
                                            joints[8] = body.Joints[JointType.ElbowRight];
                                            joints[9] = body.Joints[JointType.ShoulderRight];
    
                                            joints[10] = body.Joints[JointType.Head];
                                            joints[11] = body.Joints[JointType.Neck];
                                            joints[12] = body.Joints[JointType.SpineShoulder];
                                            joints[13] = body.Joints[JointType.SpineMid];
                                            joints[14] = body.Joints[JointType.SpineBase];
                                            joints[15] = body.Joints[JointType.HipLeft];
                                            joints[16] = body.Joints[JointType.KneeLeft];
                                            joints[17] = body.Joints[JointType.AnkleLeft];
                                            joints[18] = body.Joints[JointType.FootLeft];
    
                                            joints[19] = body.Joints[JointType.HipRight];
                                            joints[20] = body.Joints[JointType.KneeRight];
                                            joints[21] = body.Joints[JointType.AnkleRight];
                                            joints[22] = body.Joints[JointType.FootRight];
    
                                            joints[23] = body.Joints[JointType.ThumbLeft];
                                            joints[24] = body.Joints[JointType.ThumbRight];
    
                                            int cnt = 0;
                                            foreach (var join in joints)
                                            {
                                                cameraSpacePoint[cnt].X = join.Position.X;
                                                cameraSpacePoint[cnt].Y = join.Position.Y;
                                                cameraSpacePoint[cnt].Z = join.Position.Z;
                                                cnt++;
                                            }
                                            sensor.CoordinateMapper.MapCameraPointsToDepthSpace(cameraSpacePoint, depthSpacePoint);
                                            for (int i = 0; i < depthSpacePoint.Length;i++)
                                            {
                                                //using (System.IO.StreamWriter file = new System.IO.StreamWriter(@TextBlock_saveloca.Text.ToString() + "\\bodypoints.txt"))
                                                //{
                                                //    if (Record)
                                                //    {
                                                //        file.Write(string.Format("%f, %f, %f\n", depthSpacePoint[i].X, depthSpacePoint[i].Y, cameraSpacePoint[i].Z));
                                                //    }
                                                //}
                                                bodyColor.DrawCircle((int)depthSpacePoint[i].X, (int)depthSpacePoint[i].Y, 2, col, 5);
                                                if (i != 4 && i != 9 && i != 18 && i != 22 && i != 23 && i != 24)
                                                    bodyColor.DrawLine((int)depthSpacePoint[i].X, (int)depthSpacePoint[i].Y, (int)depthSpacePoint[i + 1].X, (int)depthSpacePoint[i + 1].Y, col, 2);
                                            }
                                            bodyColor.DrawLine((int)depthSpacePoint[4].X, (int)depthSpacePoint[4].Y, (int)depthSpacePoint[12].X, (int)depthSpacePoint[12].Y, col, 2);
                                            bodyColor.DrawLine((int)depthSpacePoint[9].X, (int)depthSpacePoint[9].Y, (int)depthSpacePoint[12].X, (int)depthSpacePoint[12].Y, col, 2);
                                            bodyColor.DrawLine((int)depthSpacePoint[14].X, (int)depthSpacePoint[14].Y, (int)depthSpacePoint[19].X, (int)depthSpacePoint[19].Y, col, 2);
                                            bodyColor.DrawLine((int)depthSpacePoint[23].X, (int)depthSpacePoint[23].Y, (int)depthSpacePoint[1].X, (int)depthSpacePoint[1].Y, col, 2);
                                            bodyColor.DrawLine((int)depthSpacePoint[24].X, (int)depthSpacePoint[24].Y, (int)depthSpacePoint[6].X, (int)depthSpacePoint[6].Y, col, 2);
    
                                        }
                                    }
                                    cntbodies++;
                                    if (cntbodies >= bodyframe.BodyFrameSource.BodyCount) cntbodies = 0;
                                }
                            }
                            if (CheckBox_saveB.IsChecked == true)
                            {
                                Cv.ShowImage("Body Tracking", bodyColor);
                            }
                        }
                        
                        Thread RecordingThread = new Thread(new ThreadStart(Recording));
    
                        if (Record)
                        {
                            RecordingThread.Start();                        
                        }
                        //else
                        {
                        //    RecordingThread.DisableComObjectEagerCleanup();
                            //WbColor.WritePixels(new Int32Rect(0, 0, CWidth, CHeight), Colordata, strideCol, 0);
                            //ImageColor.Source = WbColor;
                            WbDepth.WritePixels(new Int32Rect(0, 0, DWidth, DHeight), Depthdata, strideDep, 0);
                            ImageDepth.Source = WbDepth;
                            WbDepthAddCol.WritePixels(new Int32Rect(0, 0, DWidth, DHeight), DepthdataAddCol, strideDepAddCol, 0);
                            ImageColor.Source = WbDepthAddCol;
                            WbIR.WritePixels(new Int32Rect(0, 0, IRWidth, IRHeight), IRdata, strideIR, 0);
                            ImageIR.Source = WbIR;
                        }
                    }
                    catch (Exception exc)
                    {
                        //MessageBox.Show(exc.Message);
                        //Application.Current.Shutdown();
                    }
                }
            }
    
            private void Recording()
            {
                try
                {
                    if (color != null && ImageWriter.FileName != null) Cv.WriteFrame(ImageWriter, color);
                }
                catch
                { }
                
                try
                {
                    if (IR != null && IRWriter.FileName != null) Cv.WriteFrame(IRWriter, IR);
                }
                catch
                { }
                
                try
                {
                    if (depth != null && DepthWriter.FileName != null) Cv.WriteFrame(DepthWriter, depth);
                }
                catch
                { }
                    
                try
                {
                    if (CropColor != null && ImgCropWriter.FileName != null) Cv.WriteFrame(ImgCropWriter, CropColor);
                }
                catch
                { }
                try
                {
                    if (bodyColor != null && BodyWriter.FileName != null) Cv.WriteFrame(BodyWriter, bodyColor);
                }
                catch
                { }
                
            }
            private void Window_Closed(object sender, EventArgs e)
            {
                Cv.ReleaseVideoWriter(DepthWriter);
                Cv.ReleaseVideoWriter(IRWriter);
                Cv.ReleaseVideoWriter(ImgCropWriter);
                Cv.ReleaseVideoWriter(ImageWriter);
                Cv.ReleaseVideoWriter(BodyWriter);
            }
    
            private void ButtonRecord_Click(object sender, RoutedEventArgs e)
            {
                try
                {
                    if (!Record)
                    {
                        ButtonRecord.Content = "Recording";
    
                        DirectoryInfo di = new DirectoryInfo(TextBlock_saveloca.Text.ToString());
                        if (di.Exists == false)
                        {
                            di.Create();
                        }
    
                        if (CheckBox_saveD.IsChecked == true)
                            DepthWriter = new CvVideoWriter(string.Format("{1}\\Scene{0}_DepthRecord.avi", scene, TextBlock_saveloca.Text.ToString()), FourCC.Default, 30.0f, new CvSize(512, 424));
                        if (CheckBox_saveIR.IsChecked == true)
                            IRWriter = new CvVideoWriter(string.Format("{1}\\Scene{0}_IRRecord.avi", scene, TextBlock_saveloca.Text.ToString()), FourCC.Default, 30.0f, new CvSize(512, 424));
                        if (CheckBox_saveCC.IsChecked == true)
                            ImgCropWriter = new CvVideoWriter(string.Format("{1}\\Scene{0}_ImgCropRecord.avi", scene, TextBlock_saveloca.Text.ToString()), FourCC.Default, 30.0f, new CvSize(512, 424));
                        if (CheckBox_saveC.IsChecked == true)
                            ImageWriter = new CvVideoWriter(string.Format("{1}\\Scene{0}_ImageRecord.avi", scene, TextBlock_saveloca.Text.ToString()), FourCC.Default, 30.0f, new CvSize(1920, 1080));
                        if (CheckBox_saveB.IsChecked == true)
                            BodyWriter = new CvVideoWriter(string.Format("{1}\\Scene{0}_BodyRecord.avi", scene, TextBlock_saveloca.Text.ToString()), FourCC.Default, 30.0f, new CvSize(512, 424));
    
                        CheckBox_saveD.IsEnabled = false;
                        CheckBox_saveIR.IsEnabled = false;
                        CheckBox_saveCC.IsEnabled = false;
                        CheckBox_saveC.IsEnabled = false;
                        CheckBox_saveB.IsEnabled = false;
                        
                        scene++;
                        Record = true;
                        ButtonRecord.Focus();                    
                    }
                    else if (Record)
                    {
                        ButtonRecord.Content = "Ready to Record";
                        Record = false;
                        if (CheckBox_saveD.IsChecked == true) Cv.ReleaseVideoWriter(DepthWriter);
                        if (CheckBox_saveIR.IsChecked == true) Cv.ReleaseVideoWriter(IRWriter);
                        if (CheckBox_saveCC.IsChecked == true) Cv.ReleaseVideoWriter(ImgCropWriter);
                        if (CheckBox_saveC.IsChecked == true) Cv.ReleaseVideoWriter(ImageWriter);
                        if (CheckBox_saveB.IsChecked == true) Cv.ReleaseVideoWriter(BodyWriter);
                        
    
                        CheckBox_saveC.IsEnabled = true;
                        CheckBox_saveCC.IsEnabled = true;
                        CheckBox_saveIR.IsEnabled = true;
                        CheckBox_saveD.IsEnabled = true;
                        CheckBox_saveB.IsEnabled = true;
                    }
                }
                catch
                {
                    ButtonRecord.Content = "Ready to Record";
                }
            }
        }
    
    }
    

    Saturday, April 11, 2015 10:03 AM

All replies

  • To get body data from Kinect you would create a BodyReader and read the data from the BodyFrame data. Considering the data i just a points, how you would save that as an image frame or raw points would be left to you as the developer. You would follow the same programming pattern as you have used for the other readers.

    Carmine Sirignano - MSFT

    Wednesday, April 15, 2015 6:02 PM
  • I am getting this error can anyone help me to solve this?

    Sunday, April 26, 2015 12:22 AM