Пример #1
0
        // --- body
        private void ProcessBodyData(MultiFrame multiFrame)
        {
            // NOTES (!!)
            // we assume there is only one body
            // we assume joints are always in the same order
            // 12 values per joint   -> trackingStatus, posX, posY, posZ, depthX, depthY, colorX, colorY, orientX, orientY, orientZ, orientW
            // joint trackingstate enum: tracked = 2, nottracked = 0, inffered = 1?

            Body[] bodies = multiFrame.Bodies;
            Body   body   = null;

            foreach (var b in bodies)
            {
                if (b.IsTracked)
                {
                    body = b;
                    break;
                }
            }

            if (body == null)
            {
                multiFrame.BodyData = new float[25 * 12];       // return zeros if not found
                return;
            }

            int jointsCount = body.Joints.Count;

            float[] bodyData = new float[jointsCount * 12];
            int     idx      = 0;

            foreach (var kvp in body.Joints)
            {
                var jointType        = kvp.Key;
                var joint            = kvp.Value;
                var jointOrientation = body.JointOrientations[jointType];

                CameraSpacePoint position = joint.Position;
                if (position.Z < 0)
                {
                    position.Z = 0.1f;                          // according to Kinect code sample (sometimes z < 0 and the mapping return -infinity)
                }
                DepthSpacePoint depthSpacePoint = coordMapper.MapCameraPointToDepthSpace(position);
                ColorSpacePoint colorSpacePoint = coordMapper.MapCameraPointToColorSpace(position);

                bodyData[idx++] = (int)joint.TrackingState;
                bodyData[idx++] = joint.Position.X;
                bodyData[idx++] = joint.Position.Y;
                bodyData[idx++] = joint.Position.Z;
                bodyData[idx++] = depthSpacePoint.X;
                bodyData[idx++] = depthSpacePoint.Y;
                bodyData[idx++] = colorSpacePoint.X;
                bodyData[idx++] = colorSpacePoint.Y;
                bodyData[idx++] = jointOrientation.Orientation.X;
                bodyData[idx++] = jointOrientation.Orientation.Y;
                bodyData[idx++] = jointOrientation.Orientation.Z;
                bodyData[idx++] = jointOrientation.Orientation.W;
            }
            multiFrame.BodyData = bodyData;
        }
Пример #2
0
        private void AddFrame(MultiFrame multiFrame)
        {
            lock (lockObj)              // sync
            {
                if (videoWriterKinectColor != null && videoWriterKinectColor.IsOpen)
                {
                    videoWriterKinectColor.WriteVideoFrame(multiFrame.ColorResizedBitmap);
                }

                if (videoWriterKinectDepth != null && videoWriterKinectDepth.IsOpen)
                {
                    videoWriterKinectDepth.WriteVideoFrame(multiFrame.DepthBitmap);
                }

                if (bufferBody != null)
                {
                    bufferBody.Add(multiFrame.BodyData);
                }
                multiFrame.BodyData = null;            // otherwise it will keep the frame

                if (videoWriterPS3Eye != null && videoWriterPS3Eye.IsOpen)
                {
                    videoWriterPS3Eye.WriteVideoFrame(multiFrame.PS3EyeBitmap);
                }
            }
        }
Пример #3
0
        // --- depth
        private void ProcessDepthData(MultiFrame multiFrame)
        {
            byte[] depthBytes = new byte[depthWidth * depthHeight * 4 * 2];     // joined -> height x2
            int    len        = multiFrame.DepthData.Length;

            unsafe
            {
                fixed(ushort *fixedDepthData = multiFrame.DepthData)
                fixed(byte *fixedBodyIndexData = multiFrame.BodyIndexData)
                fixed(byte *fixedDepthBytes    = depthBytes)
                {
                    ushort *ptrDepthData     = fixedDepthData;
                    byte *  ptrBodyIndexData = fixedBodyIndexData;
                    byte *  ptrDepthBytes    = fixedDepthBytes;

                    // 8 bit depth
                    if (Context.Use8bitDepth)
                    {
                        for (int i = 0; i < len; i++)
                        {
                            // this is to show full depth resolution in selected range
                            //byte val = 0;
                            //ushort uVal = *ptrDepthData;
                            //if (uVal > 1000 && uVal < 1255) val = (byte)(uVal - 1000);

                            // this is full range but reduced depth resolution
                            byte val = (*ptrBodyIndexData < 6 ? byte.MaxValue : (byte)(*ptrDepthData / depthToByte));

                            *ptrDepthBytes++ = val;
                            *ptrDepthBytes++ = val;
                            *ptrDepthBytes++ = val;
                            *ptrDepthBytes++ = byte.MaxValue;        // alpha channel

                            ptrDepthData++;
                            ptrBodyIndexData++;
                        }
                    }
                    // full depth (13 bit)
                    else
                    {
                        for (int i = 0; i < len; i++)
                        {
                            // optimized for raw (faster processing)
                            *ptrDepthBytes++ = (byte)(*ptrDepthData % byte.MaxValue);
                            *ptrDepthBytes++ = (byte)(*ptrDepthData / byte.MaxValue);

                            // body & alpha
                            *ptrDepthBytes++ = (*ptrBodyIndexData < 6 ? *ptrBodyIndexData : byte.MaxValue); // assume one person only !!
                            *ptrDepthBytes++ = byte.MaxValue;                                               // alpha channel

                            ptrDepthData++;
                            ptrBodyIndexData++;
                        }
                    }
                }
            }

            multiFrame.DepthBytes = depthBytes;    // !! (otherwise GC will remove it before saving bitmap to disk)
        }
Пример #4
0
 public void EnqueueMultiFrame(MultiFrame multiFrame)
 {
     if (setNextFrameAsLastToRecord)
     {
         setNextFrameAsLastToRecord = false;
         multiFrame.IsLastRecorded  = true;
     }
     ThreadPool.QueueUserWorkItem(new WaitCallback(o => { ProcessFrame(multiFrame); }));
 }
Пример #5
0
 private void ProcessDepthBitmap(MultiFrame multiFrame)
 {
     if (Context.ProcessDepthMapping)
     {
         multiFrame.DepthBitmap = Utils.CreateBitmap(multiFrame.DepthBytes,
                                                     depthWidth, depthHeight * 2, depthWidth * 4, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
     }
     else
     {
         multiFrame.DepthBitmap = Utils.CreateBitmap(multiFrame.DepthBytes,
                                                     depthWidth, depthHeight, depthWidth * 4, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
     }
 }
Пример #6
0
 private void ProcessColorBitmap(MultiFrame multiFrame)
 {
     if (Context.ProcessColorMapping)
     {
         multiFrame.ColorResizedBitmap = Utils.CreateBitmap(multiFrame.ColorResizedData,
                                                            colorResizedWidth, colorResizedHeight * 2, colorResizedWidth * 4, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
     }
     else
     {
         multiFrame.ColorResizedBitmap = Utils.CreateBitmap(multiFrame.ColorResizedData,
                                                            colorResizedWidth, colorResizedHeight, colorResizedWidth * 4, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
     }
 }
Пример #7
0
        public void StartProcessing()
        {
            BackgroundWorker bgWorkerInput = new BackgroundWorker();

            bgWorkerInput.DoWork += (s, e) =>
            {
                // loop
                while (isProcessing)
                {
                    // --- processed queue
                    MultiFrame multiFrameProcessed;
                    if (queueMultiFramesProcessed.TryDequeue(out multiFrameProcessed))
                    {
                        // insert into list (will be sorted later)
                        lstMultiFramesSorted.Add(multiFrameProcessed);
                        Utils.SetQueueCount("Sorted", lstMultiFramesSorted.Count);
                        //multiFrameProcessed = null;
                    }
                    else
                    {
                        Thread.Sleep(processingSleepTime);
                    }

                    // --- sorted list
                    lstMultiFramesSorted.Sort(new Comparison <MultiFrame>((frameA, frameB) => frameA.FrameNb.CompareTo(frameB.FrameNb)));
                    while (lstMultiFramesSorted.Count > 0 && lstMultiFramesSorted[0].FrameNb == nextFrame)
                    {
                        // enqueue for recording
                        MultiFrame multiFrameRecording = lstMultiFramesSorted[0];
                        if (isRecording)
                        {
                            queueMultiFramesRecording.Enqueue(multiFrameRecording);
                            Utils.SetQueueCount("Recording", queueMultiFramesRecording.Count);
                        }
                        if (multiFrameRecording.IsLastRecorded)
                        {
                            isRecording = false;                                                           // !!
                        }
                        lstMultiFramesSorted.RemoveAt(0);
                        nextFrame++;

                        //multiFrameRecording.Dispose();
                        //GC.Collect();
                    }
                }
            };
            bgWorkerInput.RunWorkerAsync();
        }
Пример #8
0
        private void ProcessColorResize(MultiFrame multiFrame)
        {
            // --- resize color input
            long ticksResize = DateTime.Now.Ticks;

            byte[] colorResizedData;
            if (Context.ProcessColorMapping)
            {
                colorResizedData = new byte[colorResizedWidth * colorResizedHeight * 4 * 2];  // joined -> height x2
            }
            else
            {
                colorResizedData = new byte[colorResizedWidth * colorResizedHeight * 4];
            }
            multiFrame.ColorResizedData = colorResizedData;

            BitmapSource bmpSrc = Utils.ToBitmapSource(multiFrame.ColorData, colorWidth, colorHeight, PixelFormats.Bgr32);

            // resize to 640x360
            //BitmapSource bmpSrcResized = Utils.ResizeBitmapSource(bmpSrc, colorResizedWidth, colorResizedHeight, Context.ResizeQuality);
            //bmpSrcResized.CopyPixels(colorResizedData, colorResizedWidth * 4, 0);

            // resize to 640x480
            BitmapSource bmpSrcResized = Utils.ResizeBitmapSource(bmpSrc, Context.ColorResizedWidthTmp, colorResizedHeight, Context.ResizeQuality);

            byte[] colorResizedDataTmp = new byte[Context.ColorResizedWidthTmp * colorResizedHeight * 4];
            bmpSrcResized.CopyPixels(colorResizedDataTmp, Context.ColorResizedWidthTmp * 4, 0);

            unsafe
            {
                fixed(byte *fixedColorResizedData = colorResizedData)
                fixed(byte *fixedColorResizedDataTmp = colorResizedDataTmp)
                {
                    byte *ptrDest = fixedColorResizedData;
                    byte *ptrSrc  = fixedColorResizedDataTmp;

                    ptrSrc += 107 * 4;
                    for (int h = 0; h < colorResizedHeight; h++)
                    {
                        for (int w = 0; w < colorResizedWidth; w++)
                        {
                            *ptrDest++ = *ptrSrc++;
                            *ptrDest++ = *ptrSrc++;
                            *ptrDest++ = *ptrSrc++;
                            *ptrDest++ = *ptrSrc++;
                        }
                        ptrSrc += 213 * 4;
                    }
                }
            }
            Utils.UpdateTimer("(ColorResizeInput)", ticksResize);

            // --- resize color mapped
            long ticksResizeMapped = DateTime.Now.Ticks;

            if (multiFrame.ColorMappedBytes != null)
            {
                bmpSrc = Utils.ToBitmapSource(multiFrame.ColorMappedBytes, colorWidth, colorHeight, PixelFormats.Bgr32);

                // resize to 640x360
                //bmpSrcResized = Utils.ResizeBitmapSource(bmpSrc, colorResizedWidth, colorResizedHeight, Context.ResizeQuality);
                //bmpSrcResized.CopyPixels(colorResizedData, colorResizedWidth * 4, colorResizedByteSize);

                // resize to 640x480
                bmpSrcResized       = Utils.ResizeBitmapSource(bmpSrc, Context.ColorResizedWidthTmp, colorResizedHeight, Context.ResizeQuality);
                colorResizedDataTmp = new byte[Context.ColorResizedWidthTmp * colorResizedHeight * 4];
                bmpSrcResized.CopyPixels(colorResizedDataTmp, Context.ColorResizedWidthTmp * 4, 0);

                unsafe
                {
                    fixed(byte *fixedColorResizedData = colorResizedData)
                    fixed(byte *fixedColorResizedDataTmp = colorResizedDataTmp)
                    {
                        byte *ptrDest = fixedColorResizedData + colorResizedByteSize;
                        byte *ptrSrc  = fixedColorResizedDataTmp;

                        ptrSrc += 107 * 4;
                        for (int h = 0; h < colorResizedHeight; h++)
                        {
                            for (int w = 0; w < colorResizedWidth; w++)
                            {
                                *ptrDest++ = *ptrSrc++;
                                *ptrDest++ = *ptrSrc++;
                                *ptrDest++ = *ptrSrc++;
                                *ptrDest++ = *ptrSrc++;
                            }
                            ptrSrc += 213 * 4;
                        }
                    }
                }
            }

            Utils.UpdateTimer("(ColorResizeMapped)", ticksResizeMapped);
        }
Пример #9
0
        // --- color
        private void ProcessColorMapping(MultiFrame multiFrame)
        {
            // --- color mapping from kinect
            long ticks1 = DateTime.Now.Ticks;

            DepthSpacePoint[] colorMappedToDepthPoints = new DepthSpacePoint[colorWidth * colorHeight];
            coordMapper.MapColorFrameToDepthSpace(multiFrame.DepthData, colorMappedToDepthPoints);
            Utils.UpdateTimer("(ColorMappingCoord)", ticks1);

            // --- mapped colorAsDepth -> depth
            long ticks2 = DateTime.Now.Ticks;

            byte[] colorMappedBytes = new byte[colorWidth * colorHeight * 4];

            unsafe
            {
                fixed(byte *fixedColorMapped = colorMappedBytes)
                fixed(ushort *fixedDepthData   = multiFrame.DepthData)
                fixed(byte *fixedBodyIndexData = multiFrame.BodyIndexData)
                {
                    byte *  ptrColorMapped   = fixedColorMapped;
                    ushort *ptrDepthData     = fixedDepthData;
                    byte *  ptrBodyIndexData = fixedBodyIndexData;

                    // 8 bit
                    if (Context.Use8bitDepth)
                    {
                        for (int i = 0; i < colorPixelSize; i++)
                        {
                            // checking infinity before adding + 0.5f is about 5x faster (!!)
                            float xTmp = colorMappedToDepthPoints[i].X;
                            float yTmp = colorMappedToDepthPoints[i].Y;

                            int x = float.IsInfinity(xTmp) ? -1 : (int)(xTmp + 0.5f);
                            int y = float.IsInfinity(yTmp) ? -1 : (int)(yTmp + 0.5f);

                            if (x >= 0 && x < depthWidth && y >= 0 && y < depthHeight)
                            {
                                int  idx = x + y * depthWidth;
                                byte val = (ptrBodyIndexData[idx] < 6 ? byte.MaxValue : (byte)(ptrDepthData[idx] / depthToByte));
                                *    ptrColorMapped++ = val;
                                *    ptrColorMapped++ = val;
                                *    ptrColorMapped++ = val;
                                *    ptrColorMapped++ = 255;        // alpha
                            }
                            else
                            {
                                ptrColorMapped += 4;                // 0 is default
                            }
                        }
                    }
                    // full depth (13 bit)
                    else
                    {
                        for (int i = 0; i < colorPixelSize; i++)
                        {
                            // checking infinity before adding + 0.5f is about 5x faster (!!)
                            float xTmp = colorMappedToDepthPoints[i].X;
                            float yTmp = colorMappedToDepthPoints[i].Y;

                            int x = float.IsInfinity(xTmp) ? -1 : (int)(xTmp + 0.5f);
                            int y = float.IsInfinity(yTmp) ? -1 : (int)(yTmp + 0.5f);

                            if (x >= 0 && x < depthWidth && y >= 0 && y < depthHeight)
                            {
                                int    idx = x + y * depthWidth;
                                ushort val = ptrDepthData[idx];
                                *      ptrColorMapped++ = (byte)(val % 256);
                                *      ptrColorMapped++ = (byte)(val / 256);
                                *      ptrColorMapped++ = ptrBodyIndexData[idx];
                                *      ptrColorMapped++ = 255;      // alpha
                            }
                            else
                            {
                                ptrColorMapped += 4;                // 0 is default
                            }
                        }
                    }
                }
            }
            multiFrame.ColorMappedBytes = colorMappedBytes;
            Utils.UpdateTimer("(ColorMappingBytes)", ticks2);
        }
Пример #10
0
        private void ProcessFrame(MultiFrame multiFrame)
        {
            long ticksProcessFrame = DateTime.Now.Ticks;

            if (multiFrame.HasKinectData)
            {
                // --- color
                // color mapping
                long ticksMappingColor = DateTime.Now.Ticks;
                if (Context.ProcessColorMapping)
                {
                    ProcessColorMapping(multiFrame);
                }
                Utils.UpdateTimer("ColorMapping", ticksMappingColor);

                // color resize
                long ticksColorResize = DateTime.Now.Ticks;
                if (Context.ProcessColorResize)
                {
                    ProcessColorResize(multiFrame);
                }
                Utils.UpdateTimer("ColorResize", ticksColorResize);

                // color bitmap (for recording -> not for display)
                long ticksColorBitmap = DateTime.Now.Ticks;
                if (Context.ProcessColorBitmap)
                {
                    ProcessColorBitmap(multiFrame);
                }
                Utils.UpdateTimer("ColorBitmap", ticksColorBitmap);

                // --- depth
                // depth data (depth data & bodyIndexData)
                long ticksDepthData = DateTime.Now.Ticks;
                if (Context.ProcessDepthData)
                {
                    ProcessDepthData(multiFrame);
                }
                Utils.UpdateTimer("DepthData", ticksDepthData);

                // depth mapping
                long ticksMappingDepth = DateTime.Now.Ticks;
                if (Context.ProcessDepthMapping)
                {
                    ProcessDepthMapping(multiFrame);
                }
                Utils.UpdateTimer("DepthMapping", ticksMappingDepth);

                // depth bitmap (for recording -> not for display)
                long ticksDepthBitmap = DateTime.Now.Ticks;
                if (Context.ProcessDepthBitmap)
                {
                    ProcessDepthBitmap(multiFrame);
                }
                Utils.UpdateTimer("DepthBitmap", ticksDepthBitmap);

                // --- body
                long ticksBody = DateTime.Now.Ticks;
                if (Context.ProcessBody)
                {
                    ProcessBodyData(multiFrame);
                }
                Utils.UpdateTimer("Body", ticksBody);
            }
            if (multiFrame.HasPS3EyeData)
            {
                // --- ps3eye
                long ticksPS3EyeBitmap = DateTime.Now.Ticks;
                if (Context.ProcessPS3EyeBitmap)
                {
                    ProcessPS3EyeBitmap(multiFrame);
                }
                Utils.UpdateTimer("PS3EyeBitmap", ticksPS3EyeBitmap);
            }

            // --- display & enqueue
            ThreadPool.QueueUserWorkItem((object state) => Context.GUI.DisplayBitmaps(multiFrame));
            queueMultiFramesProcessed.Enqueue(multiFrame);
            Utils.SetQueueCount("Processed", queueMultiFramesProcessed.Count);
            Utils.UpdateTimer("ProcessFrame", ticksProcessFrame);
            Utils.UpdateCounter("Processed");
        }
        private void ProcessColorResize(MultiFrame multiFrame)
        {            
            // --- resize color input
            long ticksResize = DateTime.Now.Ticks;

            byte[] colorResizedData;
            if (Context.ProcessColorMapping)
            {
                colorResizedData = new byte[colorResizedWidth * colorResizedHeight * 4 * 2];  // joined -> height x2
            }
            else
            {
                colorResizedData = new byte[colorResizedWidth * colorResizedHeight * 4];
            }
            multiFrame.ColorResizedData = colorResizedData;

            BitmapSource bmpSrc = Utils.ToBitmapSource(multiFrame.ColorData, colorWidth, colorHeight, PixelFormats.Bgr32);

            // resize to 640x360
            //BitmapSource bmpSrcResized = Utils.ResizeBitmapSource(bmpSrc, colorResizedWidth, colorResizedHeight, Context.ResizeQuality);
            //bmpSrcResized.CopyPixels(colorResizedData, colorResizedWidth * 4, 0);

            // resize to 640x480
            BitmapSource bmpSrcResized = Utils.ResizeBitmapSource(bmpSrc, Context.ColorResizedWidthTmp, colorResizedHeight, Context.ResizeQuality);
            byte[] colorResizedDataTmp = new byte[Context.ColorResizedWidthTmp * colorResizedHeight * 4];
            bmpSrcResized.CopyPixels(colorResizedDataTmp, Context.ColorResizedWidthTmp * 4, 0);

            unsafe
            {
                fixed (byte* fixedColorResizedData = colorResizedData)
                fixed (byte* fixedColorResizedDataTmp = colorResizedDataTmp)
                {
                    byte* ptrDest = fixedColorResizedData;
                    byte* ptrSrc = fixedColorResizedDataTmp;
                    ptrSrc += 107 * 4;
                    for (int h = 0; h < colorResizedHeight; h++)
                    {
                        for (int w = 0; w < colorResizedWidth; w++)
                        {
                            *ptrDest++ = *ptrSrc++;
                            *ptrDest++ = *ptrSrc++;
                            *ptrDest++ = *ptrSrc++;
                            *ptrDest++ = *ptrSrc++;
                        }
                        ptrSrc += 213 * 4;
                    }
                }
            }
            Utils.UpdateTimer("(ColorResizeInput)", ticksResize);

            // --- resize color mapped
            long ticksResizeMapped = DateTime.Now.Ticks;

            if (multiFrame.ColorMappedBytes != null)
            {
                bmpSrc = Utils.ToBitmapSource(multiFrame.ColorMappedBytes, colorWidth, colorHeight, PixelFormats.Bgr32);

                // resize to 640x360
                //bmpSrcResized = Utils.ResizeBitmapSource(bmpSrc, colorResizedWidth, colorResizedHeight, Context.ResizeQuality);
                //bmpSrcResized.CopyPixels(colorResizedData, colorResizedWidth * 4, colorResizedByteSize);

                // resize to 640x480
                bmpSrcResized = Utils.ResizeBitmapSource(bmpSrc, Context.ColorResizedWidthTmp, colorResizedHeight, Context.ResizeQuality);
                colorResizedDataTmp = new byte[Context.ColorResizedWidthTmp * colorResizedHeight * 4];
                bmpSrcResized.CopyPixels(colorResizedDataTmp, Context.ColorResizedWidthTmp * 4, 0);

                unsafe
                {
                    fixed (byte* fixedColorResizedData = colorResizedData)
                    fixed (byte* fixedColorResizedDataTmp = colorResizedDataTmp)
                    {
                        byte* ptrDest = fixedColorResizedData + colorResizedByteSize;
                        byte* ptrSrc = fixedColorResizedDataTmp;
                        ptrSrc += 107 * 4;
                        for (int h = 0; h < colorResizedHeight; h++)
                        {
                            for (int w = 0; w < colorResizedWidth; w++)
                            {
                                *ptrDest++ = *ptrSrc++;
                                *ptrDest++ = *ptrSrc++;
                                *ptrDest++ = *ptrSrc++;
                                *ptrDest++ = *ptrSrc++;
                            }
                            ptrSrc += 213 * 4;
                        }
                    }
                }
            }

            Utils.UpdateTimer("(ColorResizeMapped)", ticksResizeMapped);
        }
Пример #12
0
 // --- ps3eye
 private void ProcessPS3EyeBitmap(MultiFrame multiFrame)
 {
     multiFrame.PS3EyeBitmap = Utils.CreateBitmap(multiFrame.PS3EyeData,
                                                  psWidth, psHeight * 2, psWidth * 4, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
 }
 // --- ps3eye
 private void ProcessPS3EyeBitmap(MultiFrame multiFrame)
 {
     multiFrame.PS3EyeBitmap = Utils.CreateBitmap(multiFrame.PS3EyeData,
         psWidth, psHeight * 2, psWidth * 4, System.Drawing.Imaging.PixelFormat.Format32bppRgb);         
 }
        // --- body
        private void ProcessBodyData(MultiFrame multiFrame)
        {
            // NOTES (!!)
            // we assume there is only one body 
            // we assume joints are always in the same order
            // 12 values per joint   -> trackingStatus, posX, posY, posZ, depthX, depthY, colorX, colorY, orientX, orientY, orientZ, orientW
            // joint trackingstate enum: tracked = 2, nottracked = 0, inffered = 1?

            Body[] bodies = multiFrame.Bodies;
            Body body = null;
            foreach (var b in bodies)
            {
                if (b.IsTracked)
                {
                    body = b;
                    break;
                }
            }

            if (body == null)
            {
                multiFrame.BodyData = new float[25 * 12];       // return zeros if not found
                return;
            }
            
            int jointsCount = body.Joints.Count;
            float[] bodyData = new float[jointsCount * 12];
            int idx = 0;
            
            foreach (var kvp in body.Joints)            
            {
                var jointType = kvp.Key;        
                var joint = kvp.Value;
                var jointOrientation = body.JointOrientations[jointType];

                CameraSpacePoint position = joint.Position;
                if (position.Z < 0) position.Z = 0.1f;          // according to Kinect code sample (sometimes z < 0 and the mapping return -infinity)

                DepthSpacePoint depthSpacePoint = coordMapper.MapCameraPointToDepthSpace(position);
                ColorSpacePoint colorSpacePoint = coordMapper.MapCameraPointToColorSpace(position);

                bodyData[idx++] = (int)joint.TrackingState;
                bodyData[idx++] = joint.Position.X;
                bodyData[idx++] = joint.Position.Y;
                bodyData[idx++] = joint.Position.Z;
                bodyData[idx++] = depthSpacePoint.X;
                bodyData[idx++] = depthSpacePoint.Y;
                bodyData[idx++] = colorSpacePoint.X;
                bodyData[idx++] = colorSpacePoint.Y;
                bodyData[idx++] = jointOrientation.Orientation.X;
                bodyData[idx++] = jointOrientation.Orientation.Y;
                bodyData[idx++] = jointOrientation.Orientation.Z;
                bodyData[idx++] = jointOrientation.Orientation.W;
            }
            multiFrame.BodyData = bodyData;
        }
 private void ProcessDepthBitmap(MultiFrame multiFrame)
 {
     if (Context.ProcessDepthMapping)
     {
         multiFrame.DepthBitmap = Utils.CreateBitmap(multiFrame.DepthBytes,
             depthWidth, depthHeight * 2, depthWidth * 4, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
     }
     else
     {
         multiFrame.DepthBitmap = Utils.CreateBitmap(multiFrame.DepthBytes,
             depthWidth, depthHeight, depthWidth * 4, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
     }            
 }
        private void ProcessDepthMapping(MultiFrame multiFrame)
        {
            // if depth data was not run
            if (multiFrame.DepthBytes == null) multiFrame.DepthBytes = new byte[depthWidth * depthHeight * 4 * 2];

            // --- depth mapping from kinect
            long ticks1 = DateTime.Now.Ticks;
            ColorSpacePoint[] depthMapping = new ColorSpacePoint[depthWidth * depthHeight];
            coordMapper.MapDepthFrameToColorSpace(multiFrame.DepthData, depthMapping);
            Utils.UpdateTimer("(DepthMappingCoord)", ticks1);

            // --- mapped depthAsColor -> color
            long ticks2 = DateTime.Now.Ticks;            
            unsafe
            {
                fixed (byte* fixedDepthMapped = multiFrame.DepthBytes)
                fixed (byte* fixedColorData = multiFrame.ColorData)
                {
                    byte* ptrDepthMapped = fixedDepthMapped;
                    ptrDepthMapped += depthByteSize * 4;                            // joined
                    byte* ptrColorData = fixedColorData;

                    int countMapped = 0;
                    int countNotMapped = 0;
                    for (int i = 0; i < depthByteSize; i++)
                    {
                        // checking infinity before adding + 0.5f is about 5x faster (!!)
                        float xTmp = depthMapping[i].X;
                        float yTmp = depthMapping[i].Y;

                        int x = float.IsInfinity(xTmp) ? -1 : (int)(xTmp + 0.5f);
                        int y = float.IsInfinity(yTmp) ? -1 : (int)(yTmp + 0.5f);

                        if (x >= 0 && x < colorWidth && y >= 0 && y < colorHeight)
                        {                            
                            int idxBase = x * 4 + y * colorWidth * 4;
                            *ptrDepthMapped++ = ptrColorData[idxBase];
                            *ptrDepthMapped++ = ptrColorData[idxBase + 1];
                            *ptrDepthMapped++ = ptrColorData[idxBase + 2];
                            *ptrDepthMapped++ = ptrColorData[idxBase + 3];
                            countMapped++;
                        }
                        else
                        {
                            *ptrDepthMapped++ = 0;
                            *ptrDepthMapped++ = 0;
                            *ptrDepthMapped++ = 0;
                            *ptrDepthMapped++ = 0;
                            countNotMapped++;
                        }
                    }
                }                
            }                                    
            Utils.UpdateTimer("(DepthMappingBytes)", ticks2);            
        }
        // --- depth
        private void ProcessDepthData(MultiFrame multiFrame)
        {            
            byte[] depthBytes = new byte[depthWidth * depthHeight * 4 * 2];     // joined -> height x2
            int len = multiFrame.DepthData.Length;
            unsafe
            {
                fixed (ushort* fixedDepthData = multiFrame.DepthData)
                fixed (byte* fixedBodyIndexData = multiFrame.BodyIndexData)
                fixed (byte* fixedDepthBytes = depthBytes) 
                {
                    ushort* ptrDepthData = fixedDepthData;
                    byte* ptrBodyIndexData = fixedBodyIndexData;
                    byte* ptrDepthBytes = fixedDepthBytes;

                    // 8 bit depth
                    if (Context.Use8bitDepth)
                    {
                        for (int i = 0; i < len; i++)
                        {
                            // this is to show full depth resolution in selected range
                            //byte val = 0;
                            //ushort uVal = *ptrDepthData;
                            //if (uVal > 1000 && uVal < 1255) val = (byte)(uVal - 1000);

                            // this is full range but reduced depth resolution
                            byte val = (*ptrBodyIndexData < 6 ? byte.MaxValue : (byte)(*ptrDepthData / depthToByte));                            

                            *ptrDepthBytes++ = val;
                            *ptrDepthBytes++ = val;
                            *ptrDepthBytes++ = val;
                            *ptrDepthBytes++ = byte.MaxValue;        // alpha channel

                            ptrDepthData++;
                            ptrBodyIndexData++;
                        }
                    }
                    // full depth (13 bit)
                    else
                    {
                        for (int i = 0; i < len; i++)
                        {
                            // optimized for raw (faster processing)
                            *ptrDepthBytes++ = (byte)(*ptrDepthData % byte.MaxValue);
                            *ptrDepthBytes++ = (byte)(*ptrDepthData / byte.MaxValue);
                            
                            // body & alpha
                            *ptrDepthBytes++ = (*ptrBodyIndexData < 6 ? *ptrBodyIndexData : byte.MaxValue);         // assume one person only !!
                            *ptrDepthBytes++ = byte.MaxValue;        // alpha channel

                            ptrDepthData++;
                            ptrBodyIndexData++;
                        }
                    }
                }
            }

            multiFrame.DepthBytes = depthBytes;    // !! (otherwise GC will remove it before saving bitmap to disk)            
        }
 private void ProcessColorBitmap(MultiFrame multiFrame)
 {
     if (Context.ProcessColorMapping)
     {
         multiFrame.ColorResizedBitmap = Utils.CreateBitmap(multiFrame.ColorResizedData,
             colorResizedWidth, colorResizedHeight * 2, colorResizedWidth * 4, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
     }
     else
     {
         multiFrame.ColorResizedBitmap = Utils.CreateBitmap(multiFrame.ColorResizedData,
             colorResizedWidth, colorResizedHeight, colorResizedWidth * 4, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
     }
 }
        private void ProcessFrame(MultiFrame multiFrame)
        {
            long ticksProcessFrame = DateTime.Now.Ticks;

            if (multiFrame.HasKinectData)
            {
                // --- color
                // color mapping
                long ticksMappingColor = DateTime.Now.Ticks;
                if (Context.ProcessColorMapping) ProcessColorMapping(multiFrame);
                Utils.UpdateTimer("ColorMapping", ticksMappingColor);

                // color resize
                long ticksColorResize = DateTime.Now.Ticks;
                if (Context.ProcessColorResize) ProcessColorResize(multiFrame);
                Utils.UpdateTimer("ColorResize", ticksColorResize);

                // color bitmap (for recording -> not for display)
                long ticksColorBitmap = DateTime.Now.Ticks;
                if (Context.ProcessColorBitmap) ProcessColorBitmap(multiFrame);
                Utils.UpdateTimer("ColorBitmap", ticksColorBitmap);

                // --- depth
                // depth data (depth data & bodyIndexData)
                long ticksDepthData = DateTime.Now.Ticks;
                if (Context.ProcessDepthData) ProcessDepthData(multiFrame);
                Utils.UpdateTimer("DepthData", ticksDepthData);

                // depth mapping
                long ticksMappingDepth = DateTime.Now.Ticks;
                if (Context.ProcessDepthMapping) ProcessDepthMapping(multiFrame);
                Utils.UpdateTimer("DepthMapping", ticksMappingDepth);

                // depth bitmap (for recording -> not for display)
                long ticksDepthBitmap = DateTime.Now.Ticks;
                if (Context.ProcessDepthBitmap) ProcessDepthBitmap(multiFrame);
                Utils.UpdateTimer("DepthBitmap", ticksDepthBitmap);

                // --- body
                long ticksBody = DateTime.Now.Ticks;
                if (Context.ProcessBody) ProcessBodyData(multiFrame);
                Utils.UpdateTimer("Body", ticksBody);
            }
            if (multiFrame.HasPS3EyeData)
            {
                // --- ps3eye
                long ticksPS3EyeBitmap = DateTime.Now.Ticks;
                if (Context.ProcessPS3EyeBitmap) ProcessPS3EyeBitmap(multiFrame);
                Utils.UpdateTimer("PS3EyeBitmap", ticksPS3EyeBitmap);
            }

            // --- display & enqueue
            ThreadPool.QueueUserWorkItem((object state) => Context.GUI.DisplayBitmaps(multiFrame));
            queueMultiFramesProcessed.Enqueue(multiFrame);
            Utils.SetQueueCount("Processed", queueMultiFramesProcessed.Count);
            Utils.UpdateTimer("ProcessFrame", ticksProcessFrame);
            Utils.UpdateCounter("Processed");
        }
Пример #20
0
        private void ProcessDepthMapping(MultiFrame multiFrame)
        {
            // if depth data was not run
            if (multiFrame.DepthBytes == null)
            {
                multiFrame.DepthBytes = new byte[depthWidth * depthHeight * 4 * 2];
            }

            // --- depth mapping from kinect
            long ticks1 = DateTime.Now.Ticks;

            ColorSpacePoint[] depthMapping = new ColorSpacePoint[depthWidth * depthHeight];
            coordMapper.MapDepthFrameToColorSpace(multiFrame.DepthData, depthMapping);
            Utils.UpdateTimer("(DepthMappingCoord)", ticks1);

            // --- mapped depthAsColor -> color
            long ticks2 = DateTime.Now.Ticks;

            unsafe
            {
                fixed(byte *fixedDepthMapped = multiFrame.DepthBytes)
                fixed(byte *fixedColorData = multiFrame.ColorData)
                {
                    byte *ptrDepthMapped = fixedDepthMapped;

                    ptrDepthMapped += depthByteSize * 4;                            // joined
                    byte *ptrColorData = fixedColorData;

                    int countMapped    = 0;
                    int countNotMapped = 0;

                    for (int i = 0; i < depthByteSize; i++)
                    {
                        // checking infinity before adding + 0.5f is about 5x faster (!!)
                        float xTmp = depthMapping[i].X;
                        float yTmp = depthMapping[i].Y;

                        int x = float.IsInfinity(xTmp) ? -1 : (int)(xTmp + 0.5f);
                        int y = float.IsInfinity(yTmp) ? -1 : (int)(yTmp + 0.5f);

                        if (x >= 0 && x < colorWidth && y >= 0 && y < colorHeight)
                        {
                            int idxBase          = x * 4 + y * colorWidth * 4;
                            *   ptrDepthMapped++ = ptrColorData[idxBase];
                            *   ptrDepthMapped++ = ptrColorData[idxBase + 1];
                            *   ptrDepthMapped++ = ptrColorData[idxBase + 2];
                            *   ptrDepthMapped++ = ptrColorData[idxBase + 3];
                            countMapped++;
                        }
                        else
                        {
                            *ptrDepthMapped++ = 0;
                            *ptrDepthMapped++ = 0;
                            *ptrDepthMapped++ = 0;
                            *ptrDepthMapped++ = 0;
                            countNotMapped++;
                        }
                    }
                }
            }
            Utils.UpdateTimer("(DepthMappingBytes)", ticks2);
        }
        private void bgWorkerSupervisor_DoWork(object sender, DoWorkEventArgs e)
        {
            int frameWidth  = devices[0].FrameWidth;
            int frameHeight = devices[0].FrameHeight;
            int frameStride = devices[0].FrameStride;
            int imgSize     = devices[0].ImgSize;

            long ticksNextAcq = DateTime.Now.Ticks + 5 * TimeSpan.TicksPerMillisecond;      // initial value
            int  fps          = Context.GUI.PS3EyePanel.FrameRate;
            int  frameTime    = 1000 / fps;

            while (isPs3EyeRunning)
            {
                ticksNextAcq = ticksNextAcq + frameTime * TimeSpan.TicksPerMillisecond;
                while (DateTime.Now.Ticks < ticksNextAcq)
                {
                    Thread.Sleep(1);
                }

                // signal other threads to acquire frames
                if (barrier != null)
                {
                    try { barrier.SignalAndWait(cancelToken); } catch { }
                }
                Utils.UpdateCounter("Input");
                long ticksAcqTotal = DateTime.Now.Ticks;

                // frames acquired here (other threads)

                // signal other threads to acquire frames
                if (barrier != null)
                {
                    try { barrier.SignalAndWait(cancelToken); } catch { }
                }
                Utils.UpdateCounter("Acquired");
                Utils.UpdateTimer("Acquire", ticksAcqTotal);
                Utils.UpdateCounter("Expired", false);

                long ticksCopyData = DateTime.Now.Ticks;

                // now first frames are synchronized
                byte[] bytes0 = frames[0];
                byte[] bytes1 = frames[1];

                // join frames
                byte[] psBytes = null;
                if (bytes0 != null && bytes1 != null)
                {
                    long ticksCreatePS3EyeData = DateTime.Now.Ticks;
                    psBytes = new byte[psByteSize * 2];
                    Utils.UpdateTimer("CreatePS3EyeData", ticksCreatePS3EyeData);

                    long ticksCopyPS3EyeData = DateTime.Now.Ticks;
                    CopyPS3EyeDataMirror(psBytes, bytes0, bytes1);      // mirror to be consistent with Kinect 2
                    Utils.UpdateTimer("CopyPS3EyeData", ticksCopyPS3EyeData);
                }

                // multiFrame
                long       ticksMultiFrame = DateTime.Now.Ticks;
                MultiFrame multiFrame      = new MultiFrame();
                multiFrame.FrameNb       = Interlocked.Increment(ref frameNb);
                multiFrame.PS3EyeData    = psBytes;
                multiFrame.HasKinectData = false;
                multiFrame.HasPS3EyeData = true;
                Utils.UpdateTimer("MultiFrame", ticksMultiFrame);

                long ticksEnqueue = DateTime.Now.Ticks;
                ProcessingManager.Instance.EnqueueMultiFrame(multiFrame);
                Utils.UpdateTimer("Enqueue", ticksEnqueue);

                Utils.UpdateTimer("CopyFramesData", ticksCopyData);

                // display timers & queues
                Context.GUI.DisplayPerformance();
            }
            // each camera thread disposes its cam
        }
 public void EnqueueMultiFrame(MultiFrame multiFrame)
 {            
     if (setNextFrameAsLastToRecord)
     {
         setNextFrameAsLastToRecord = false;
         multiFrame.IsLastRecorded = true;
     }
     ThreadPool.QueueUserWorkItem(new WaitCallback(o => { ProcessFrame(multiFrame); }));
 }
        private void ProcessFrames(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame, BodyFrame bodyFrame, byte [] psBytes0, byte [] psBytes1)
        {            
            // create multiframe to process
            long ticksCopyData = DateTime.Now.Ticks;

            MultiFrame multiFrame = new MultiFrame();
            multiFrame.FrameNb = Interlocked.Increment(ref frameNb);

            // color
            long ticksCreateColorData = DateTime.Now.Ticks;
            byte[] colorData = new byte[colorByteSize];
            Utils.UpdateTimer("CreateColorData", ticksCreateColorData);

            long ticksCopyColorData = DateTime.Now.Ticks;
            colorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Bgra);
            Utils.UpdateTimer("CopyColorData", ticksCopyColorData);

            // depth
            long ticksCreateDepthData = DateTime.Now.Ticks;
            ushort[] depthData = new ushort[depthPixelSize];
            depthFrame.CopyFrameDataToArray(depthData);            
            Utils.UpdateTimer("CreateDepthData", ticksCreateDepthData);

            // body index
            long ticksCreateBodyIndexData = DateTime.Now.Ticks;
            byte[] bodyIndexData = new byte[depthPixelSize];
            bodyIndexFrame.CopyFrameDataToArray(bodyIndexData);
            Utils.UpdateTimer("CreateBodyIndexData", ticksCreateBodyIndexData);

            // bodies
            long ticksCreateBodiesData = DateTime.Now.Ticks;
            Body[] bodies = new Body[bodyFrame.BodyCount];
            bodyFrame.GetAndRefreshBodyData(bodies);
            Utils.UpdateTimer("CreateBodiesData", ticksCreateBodiesData);

            // ps3eye
            byte[] psBytes = null;
            if (psBytes0 != null && psBytes1 != null)
            {
                long ticksCreatePS3EyeData = DateTime.Now.Ticks;
                psBytes = new byte[psByteSize * 2];
                Utils.UpdateTimer("CreatePS3EyeData", ticksCreatePS3EyeData);

                long ticksCopyPS3EyeData = DateTime.Now.Ticks;
                CopyPS3EyeDataMirror(psBytes, psBytes0, psBytes1);
                Utils.UpdateTimer("CopyPS3EyeData", ticksCopyPS3EyeData);
            }

            // multiFrame
            long ticksMultiFrame = DateTime.Now.Ticks;
            multiFrame.DepthData = depthData;
            multiFrame.ColorData = colorData;
            multiFrame.BodyIndexData = bodyIndexData;
            multiFrame.Bodies = bodies;
            multiFrame.PS3EyeData = psBytes;
            multiFrame.HasKinectData = true;
            multiFrame.HasPS3EyeData = psBytes != null ? true : false;
            Utils.UpdateTimer("MultiFrame", ticksMultiFrame);

            long ticksEnqueue = DateTime.Now.Ticks;
            ProcessingManager.Instance.EnqueueMultiFrame(multiFrame);
            Utils.UpdateTimer("Enqueue", ticksEnqueue);

            Utils.UpdateTimer("CopyFramesData", ticksCopyData);

            // display timers & queues
            Context.GUI.DisplayPerformance();
        }
        private void AddFrame(MultiFrame multiFrame)
        {
            lock (lockObj)              // sync
            {                                
                if (videoWriterKinectColor != null && videoWriterKinectColor.IsOpen) videoWriterKinectColor.WriteVideoFrame(multiFrame.ColorResizedBitmap);

                if (videoWriterKinectDepth != null && videoWriterKinectDepth.IsOpen) videoWriterKinectDepth.WriteVideoFrame(multiFrame.DepthBitmap);

                if (bufferBody != null) bufferBody.Add(multiFrame.BodyData);
                multiFrame.BodyData = null;            // otherwise it will keep the frame

                if (videoWriterPS3Eye != null && videoWriterPS3Eye.IsOpen) videoWriterPS3Eye.WriteVideoFrame(multiFrame.PS3EyeBitmap);
            }
        }
        // --- color
        private void ProcessColorMapping(MultiFrame multiFrame)
        {
            // --- color mapping from kinect
            long ticks1 = DateTime.Now.Ticks;
            DepthSpacePoint[] colorMappedToDepthPoints = new DepthSpacePoint[colorWidth * colorHeight];
            coordMapper.MapColorFrameToDepthSpace(multiFrame.DepthData, colorMappedToDepthPoints);            
            Utils.UpdateTimer("(ColorMappingCoord)", ticks1);

            // --- mapped colorAsDepth -> depth
            long ticks2 = DateTime.Now.Ticks;
            byte[] colorMappedBytes = new byte[colorWidth * colorHeight * 4];

            unsafe
            {
                fixed (byte* fixedColorMapped = colorMappedBytes)
                fixed (ushort* fixedDepthData = multiFrame.DepthData)
                fixed (byte* fixedBodyIndexData = multiFrame.BodyIndexData)
                {
                    byte* ptrColorMapped = fixedColorMapped;
                    ushort* ptrDepthData = fixedDepthData;
                    byte* ptrBodyIndexData = fixedBodyIndexData;

                    // 8 bit
                    if (Context.Use8bitDepth)
                    {
                        for (int i = 0; i < colorPixelSize; i++)                                               
                        {
                            // checking infinity before adding + 0.5f is about 5x faster (!!)
                            float xTmp = colorMappedToDepthPoints[i].X;
                            float yTmp = colorMappedToDepthPoints[i].Y;

                            int x = float.IsInfinity(xTmp) ? -1 : (int)(xTmp + 0.5f);
                            int y = float.IsInfinity(yTmp) ? -1 : (int)(yTmp + 0.5f);

                            if (x >= 0 && x < depthWidth && y >= 0 && y < depthHeight)
                            {
                                int idx = x + y * depthWidth;
                                byte val = (ptrBodyIndexData[idx] < 6 ? byte.MaxValue : (byte)(ptrDepthData[idx] / depthToByte));
                                *ptrColorMapped++ = val;
                                *ptrColorMapped++ = val;
                                *ptrColorMapped++ = val;
                                *ptrColorMapped++ = 255;            // alpha
                            }
                            else
                            {
                                ptrColorMapped += 4;                // 0 is default
                            }
                        }
                    }
                    // full depth (13 bit)
                    else
                    {
                        for (int i = 0; i < colorPixelSize; i++)
                        {
                            // checking infinity before adding + 0.5f is about 5x faster (!!)
                            float xTmp = colorMappedToDepthPoints[i].X;
                            float yTmp = colorMappedToDepthPoints[i].Y;

                            int x = float.IsInfinity(xTmp) ? -1 : (int)(xTmp + 0.5f);
                            int y = float.IsInfinity(yTmp) ? -1 : (int)(yTmp + 0.5f);

                            if (x >= 0 && x < depthWidth && y >= 0 && y < depthHeight)
                            {
                                int idx = x + y * depthWidth;
                                ushort val = ptrDepthData[idx];
                                *ptrColorMapped++ = (byte)(val % 256);
                                *ptrColorMapped++ = (byte)(val / 256);
                                *ptrColorMapped++ = ptrBodyIndexData[idx];
                                *ptrColorMapped++ = 255;            // alpha
                            }
                            else
                            {
                                ptrColorMapped += 4;                // 0 is default
                            }
                        }
                    }
                }
            }            
            multiFrame.ColorMappedBytes = colorMappedBytes;
            Utils.UpdateTimer("(ColorMappingBytes)", ticks2);
        }
        private void ProcessFrames(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame, BodyFrame bodyFrame, byte [] psBytes0, byte [] psBytes1)
        {
            // create multiframe to process
            long ticksCopyData = DateTime.Now.Ticks;

            MultiFrame multiFrame = new MultiFrame();

            multiFrame.FrameNb = Interlocked.Increment(ref frameNb);

            // color
            long ticksCreateColorData = DateTime.Now.Ticks;

            byte[] colorData = new byte[colorByteSize];
            Utils.UpdateTimer("CreateColorData", ticksCreateColorData);

            long ticksCopyColorData = DateTime.Now.Ticks;

            colorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Bgra);
            Utils.UpdateTimer("CopyColorData", ticksCopyColorData);

            // depth
            long ticksCreateDepthData = DateTime.Now.Ticks;

            ushort[] depthData = new ushort[depthPixelSize];
            depthFrame.CopyFrameDataToArray(depthData);
            Utils.UpdateTimer("CreateDepthData", ticksCreateDepthData);

            // body index
            long ticksCreateBodyIndexData = DateTime.Now.Ticks;

            byte[] bodyIndexData = new byte[depthPixelSize];
            bodyIndexFrame.CopyFrameDataToArray(bodyIndexData);
            Utils.UpdateTimer("CreateBodyIndexData", ticksCreateBodyIndexData);

            // bodies
            long ticksCreateBodiesData = DateTime.Now.Ticks;

            Body[] bodies = new Body[bodyFrame.BodyCount];
            bodyFrame.GetAndRefreshBodyData(bodies);
            Utils.UpdateTimer("CreateBodiesData", ticksCreateBodiesData);

            // ps3eye
            byte[] psBytes = null;
            if (psBytes0 != null && psBytes1 != null)
            {
                long ticksCreatePS3EyeData = DateTime.Now.Ticks;
                psBytes = new byte[psByteSize * 2];
                Utils.UpdateTimer("CreatePS3EyeData", ticksCreatePS3EyeData);

                long ticksCopyPS3EyeData = DateTime.Now.Ticks;
                CopyPS3EyeDataMirror(psBytes, psBytes0, psBytes1);
                Utils.UpdateTimer("CopyPS3EyeData", ticksCopyPS3EyeData);
            }

            // multiFrame
            long ticksMultiFrame = DateTime.Now.Ticks;

            multiFrame.DepthData     = depthData;
            multiFrame.ColorData     = colorData;
            multiFrame.BodyIndexData = bodyIndexData;
            multiFrame.Bodies        = bodies;
            multiFrame.PS3EyeData    = psBytes;
            multiFrame.HasKinectData = true;
            multiFrame.HasPS3EyeData = psBytes != null ? true : false;
            Utils.UpdateTimer("MultiFrame", ticksMultiFrame);

            long ticksEnqueue = DateTime.Now.Ticks;

            ProcessingManager.Instance.EnqueueMultiFrame(multiFrame);
            Utils.UpdateTimer("Enqueue", ticksEnqueue);

            Utils.UpdateTimer("CopyFramesData", ticksCopyData);

            // display timers & queues
            Context.GUI.DisplayPerformance();
        }
        private void bgWorkerSupervisor_DoWork(object sender, DoWorkEventArgs e)
        {
            int frameWidth = devices[0].FrameWidth;
            int frameHeight = devices[0].FrameHeight;
            int frameStride = devices[0].FrameStride;
            int imgSize = devices[0].ImgSize;

            long ticksNextAcq = DateTime.Now.Ticks + 5 * TimeSpan.TicksPerMillisecond;      // initial value
            int fps = Context.GUI.PS3EyePanel.FrameRate;
            int frameTime = 1000 / fps;

            while (isPs3EyeRunning)
            {                
                ticksNextAcq = ticksNextAcq + frameTime * TimeSpan.TicksPerMillisecond;
                while (DateTime.Now.Ticks < ticksNextAcq) Thread.Sleep(1);

                // signal other threads to acquire frames
                if (barrier != null) { try { barrier.SignalAndWait(cancelToken); } catch { } }
                Utils.UpdateCounter("Input");
                long ticksAcqTotal = DateTime.Now.Ticks;

                // frames acquired here (other threads)

                // signal other threads to acquire frames
                if (barrier != null) { try { barrier.SignalAndWait(cancelToken); } catch { } }
                Utils.UpdateCounter("Acquired");
                Utils.UpdateTimer("Acquire", ticksAcqTotal);
                Utils.UpdateCounter("Expired", false);

                long ticksCopyData = DateTime.Now.Ticks;

                // now first frames are synchronized
                byte[] bytes0 = frames[0];
                byte[] bytes1 = frames[1];

                // join frames
                byte[] psBytes = null;
                if (bytes0 != null && bytes1 != null)
                {
                    long ticksCreatePS3EyeData = DateTime.Now.Ticks;
                    psBytes = new byte[psByteSize * 2];
                    Utils.UpdateTimer("CreatePS3EyeData", ticksCreatePS3EyeData);

                    long ticksCopyPS3EyeData = DateTime.Now.Ticks;
                    CopyPS3EyeDataMirror(psBytes, bytes0, bytes1);      // mirror to be consistent with Kinect 2
                    Utils.UpdateTimer("CopyPS3EyeData", ticksCopyPS3EyeData);
                }

                // multiFrame
                long ticksMultiFrame = DateTime.Now.Ticks;
                MultiFrame multiFrame = new MultiFrame();
                multiFrame.FrameNb = Interlocked.Increment(ref frameNb);
                multiFrame.PS3EyeData = psBytes;
                multiFrame.HasKinectData = false;
                multiFrame.HasPS3EyeData = true;
                Utils.UpdateTimer("MultiFrame", ticksMultiFrame);

                long ticksEnqueue = DateTime.Now.Ticks;
                ProcessingManager.Instance.EnqueueMultiFrame(multiFrame);
                Utils.UpdateTimer("Enqueue", ticksEnqueue);

                Utils.UpdateTimer("CopyFramesData", ticksCopyData);

                // display timers & queues
                Context.GUI.DisplayPerformance();
            }
            // each camera thread disposes its cam    
        }