/// <summary>
        /// 
        /// </summary>
        /// <param name="colorFrame"></param>
        /// <param name="headList"></param>
        private void FillBitmap( ColorImageFrame colorFrame, List<Tuple<SkeletonPoint, Matrix4>> headList )
        {
            // 描画の準備
            using ( var drawContecxt = drawVisual.RenderOpen() ) {
                // 画像情報をバッファにコピー
                colorFrame.CopyPixelDataTo( pixelBuffer );

                // カメラの画像情報から、背景のビットマップを作成し描画する
                var backgroundImage = new WriteableBitmap( colorFrame.Width, colorFrame.Height, 96, 96,
                    PixelFormats.Bgr32, null );
                backgroundImage.WritePixels( new Int32Rect( 0, 0, colorFrame.Width, colorFrame.Height ),
                    pixelBuffer, colorFrame.Width * 4, 0 );
                drawContecxt.DrawImage( backgroundImage,
                    new Rect( 0, 0, colorFrame.Width, colorFrame.Height ) );

                // 頭の位置にマスク画像を表示する
                foreach ( var head in headList ) {
                    ColorImagePoint headPoint = kinect.MapSkeletonPointToColor( head.Item1, rgbFormat );

                    // 頭の位置の向きに回転させたマスク画像を描画する
                    Matrix4 hm = head.Item2;
                    Matrix rot = new Matrix( -hm.M11, hm.M12,
                                             -hm.M21, hm.M22,
                                             headPoint.X, headPoint.Y );
                    drawContecxt.PushTransform( new MatrixTransform( rot ) );

                    drawContecxt.DrawImage( maskImage,
                        new Rect( -64, -64, 128, 128 ) );
                    drawContecxt.Pop();
                }
            }

            // 画面に表示するビットマップに描画する
            bmpBuffer.Render( drawVisual );
        }

        public WriteableBitmap bmpFromColorFrame(ColorImageFrame NewFrame, bool SkeletonWanted, SkeletonFrame SkelFrame)
        {
            if (bmpColor == null)
            {
                bmpColor = new WriteableBitmap(frameWidth, frameHeight, 96, 96, PixelFormats.Bgr32, null);
            }

            if (NewFrame == null)
            {
                throw new InvalidOperationException("Null Image");
            }

            NewFrame.CopyPixelDataTo(colorPixels);
            // Write the pixel data into our bitmap
            bmpColor.WritePixels(
            new Int32Rect(0, 0, bmpColor.PixelWidth, bmpColor.PixelHeight),
                colorPixels,
                bmpColor.PixelWidth * sizeof(int),
                0);
            //Skeleton
            if (SkeletonWanted != false && SkelFrame != null)
            {
                return bmpWithSkelFromColor(bmpColor, SkelFrame);
            }
            return bmpColor;
        }

        public void OnAllFramesReady(ColorImageFrame colorImageFrame, DepthImageFrame depthImageFrame, SkeletonFrame skeletonFrame)
        {
            string stat = "";
            // Face tracking / timing stuff
            if (FaceList.Count > 0) {
                foreach (Face f in FaceList.toList()) {
                    stat += "    {" + f.Id + ", " + String.Format("{0:0.00}", f.Velocity) + "}";
                    if (f.TakePicture) {
                        int[] coordsExpanded = expandBy(f.Coords, 250, colorImageFrame.Width - 1, colorImageFrame.Height - 1);
                        if (coordsExpanded[2] == 0 || coordsExpanded[3] == 0) // width or height can't be 0
                            continue;
                        string time = System.DateTime.Now.ToString("hh'-'mm'-'ss", CultureInfo.CurrentUICulture.DateTimeFormat);
                        string path = "..\\..\\Images\\CroppedPics\\pic" + f.Id + "--" + time + ".jpg";
                        f.Path = path;
                        bool success = SaveImage(path, cropImage(colorImageFrame, coordsExpanded), ImageFormat.Jpeg);
                        if (success && USE_BETAFACE) {
                            bfw.enqueue(f);
                            f.ProcessingBetaface = true;

                        }
                    }

                }
            }
            FaceTracking.Status = stat;
            FaceTracking.Status = "";
        }

        public static Bitmap ColorImageFrameToBitmap(ColorImageFrame colorFrame)
        {
            byte[] pixelBuffer = new byte[colorFrame.PixelDataLength];
            colorFrame.CopyPixelDataTo(pixelBuffer);

            System.Drawing.Imaging.PixelFormat pixelFormat = System.Drawing.Imaging.PixelFormat.Format32bppRgb;

            if (colorFrame.Format == ColorImageFormat.InfraredResolution640x480Fps30)
            {
                pixelFormat = System.Drawing.Imaging.PixelFormat.Format16bppRgb565;
            }

            Bitmap bitmapFrame = new Bitmap(colorFrame.Width, colorFrame.Height, pixelFormat);
            System.Drawing.Rectangle rect = new System.Drawing.Rectangle(0, 0, bitmapFrame.Width, bitmapFrame.Height);
            BitmapData bitmapData = bitmapFrame.LockBits(rect, ImageLockMode.WriteOnly, bitmapFrame.PixelFormat);

            IntPtr intPointer = bitmapData.Scan0;
            Marshal.Copy(pixelBuffer, 0, intPointer, colorFrame.PixelDataLength);

            bitmapFrame.UnlockBits(bitmapData);

            bitmapData = null;
            pixelBuffer = null;

            return bitmapFrame;
        }

        /// <summary>
        /// Sets the data of ColorVideo.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="colorImageFrame"></param>
        public void KinectColorFrameReady(object sender, ColorImageFrameReadyEventArgs colorImageFrame)
        {
            //Get raw image
            ColorVideoFrame = colorImageFrame.OpenColorImageFrame();

            if (ColorVideoFrame != null)
            {
                //Create array for pixel data and copy it from the image frame
                PixelData = new Byte[ColorVideoFrame.PixelDataLength];
                ColorVideoFrame.CopyPixelDataTo(PixelData);

                //Convert RGBA to BGRA, Kinect and XNA uses different color-formats.
                BgraPixelData = new Byte[ColorVideoFrame.PixelDataLength];
                for (int i = 0; i < PixelData.Length; i += 4)
                {
                    BgraPixelData[i] = PixelData[i + 2];
                    BgraPixelData[i + 1] = PixelData[i + 1];
                    BgraPixelData[i + 2] = PixelData[i];
                    BgraPixelData[i + 3] = (Byte)255; //The video comes with 0 alpha so it is transparent
                }

                // Create a texture and assign the realigned pixels
                ColorVideo = new Texture2D(Graphics.GraphicsDevice, ColorVideoFrame.Width, ColorVideoFrame.Height);
                ColorVideo.SetData(BgraPixelData);
                ColorVideoFrame.Dispose();
            }
        }

        public KServerColorStreamPaquet(ColorImageFrame _imageFrame, int _idSensor)
        {
            idSensor = _idSensor;
            imageFrame = _imageFrame;

            byte[] pxlData = new byte[imageFrame.PixelDataLength];
            imageFrame.CopyPixelDataTo(pxlData);

            jpgImage = new MemoryStream();
            unsafe {
               fixed (byte* ptr = pxlData) {
                  using (Bitmap image = new Bitmap(imageFrame.Width, imageFrame.Height, imageFrame.Width*4, PixelFormat.Format32bppArgb, new IntPtr(ptr))) {
                     image.Save(jpgImage, ImageFormat.Jpeg);
                  }
               }
            }

            /* Sets the size of the paquet */
            setBodySize((uint)(jpgImage.Length+5));
            byte[] size = BitConverter.GetBytes((UInt32)(jpgImage.Length+5));

            Array.Reverse(size);
            Buffer.BlockCopy(size, 0, data, 0, size.Length);

            /* Builds the paquet */
            build();
        }

 /// <summary>
 /// Initializes a new instance of the <see cref="ColorImageFrameLuminanceSource"/> class.
 /// </summary>
 /// <param name="bitmap">The bitmap.</param>
 /// <param name="flipTheImage">if set to <c>true</c> [flip the image].</param>
 public ColorImageFrameLuminanceSource(ColorImageFrame bitmap, bool flipTheImage)
    : base(bitmap.Width, bitmap.Height)
 {
    var pixelData = new byte[bitmap.PixelDataLength];
    var bitmapFormat = BitmapFormat.Unknown;
    switch (bitmap.Format)
    {
       case ColorImageFormat.InfraredResolution640x480Fps30:
          // not sure, what BitmapFormat should be selected
          break;
       case ColorImageFormat.RawBayerResolution1280x960Fps12:
       case ColorImageFormat.RawBayerResolution640x480Fps30:
          // not sure, what BitmapFormat should be selected
          break;
       case ColorImageFormat.RgbResolution1280x960Fps12:
       case ColorImageFormat.RgbResolution640x480Fps30:
          bitmapFormat = BitmapFormat.BGR32;
          break;
       case ColorImageFormat.RawYuvResolution640x480Fps15:
       case ColorImageFormat.YuvResolution640x480Fps15:
          // not sure, what BitmapFormat should be selected
          break;
       default:
          break;
    }
    bitmap.CopyPixelDataTo(pixelData);
    CalculateLuminance(pixelData, bitmapFormat);
    if (flipTheImage)
    {
       // flip the luminance values because the kinect has it flipped before
       FlipLuminanceValues();
    }
 }

        public RecordedVideoFrame(ColorImageFrame colorFrame)
        {
            if (colorFrame != null)
            {
                byte[] bits = new byte[colorFrame.PixelDataLength];
                colorFrame.CopyPixelDataTo(bits);

                int BytesPerPixel = colorFrame.BytesPerPixel;
                int Width = colorFrame.Width;
                int Height = colorFrame.Height;

                var bmp = new WriteableBitmap(Width, Height, 96, 96, PixelFormats.Bgr32, null);
                bmp.WritePixels(new System.Windows.Int32Rect(0, 0, Width, Height), bits, Width * BytesPerPixel, 0);
                JpegBitmapEncoder jpeg = new JpegBitmapEncoder();
                jpeg.Frames.Add(BitmapFrame.Create(bmp));
                var SaveStream = new MemoryStream();
                jpeg.Save(SaveStream);
                SaveStream.Flush();
                JpegData = SaveStream.ToArray();
            }
            else
            {
                return;
            }
        }

 public void findCoordinate(ColorImageFrame frame)
 {
     int[] position = new int[2];
     Image<Bgr, Byte> img = ImageProc.colorFrameToImage(frame);
     position = ImageProc.matchImages(img, this.template);
     this.position = position;
 }

 // coords is {left, top, width, height}
 private Image cropImage(ColorImageFrame source, int[] coords)
 {
     Rectangle cropArea = new Rectangle(coords[0], coords[1], coords[2], coords[3]);
     Bitmap bmpImage = ImageToBitmap(source);
     Bitmap bmpCrop = bmpImage.Clone(cropArea,
     bmpImage.PixelFormat);
     return (Image)(bmpCrop);
 }

        public void CaptureSkeletonData(Skeleton[] skeletonData, ColorImageFrame imageFrame, DateTime timeStamp)
        {
            List<Skeleton> skeletons = RecognizeSkeletons(skeletonData);
            SkeletonCaptureData data = new SkeletonCaptureData(skeletons, imageFrame, timeStamp);

            foreach (ISkeletonCapturingFunction capturingFunction in capturingFunctions)
            {
                ExecuteCapturingFunction(capturingFunction, data);
            }
        }

        private BitmapSource ObterImagemSensorRGB(ColorImageFrame quadro)
        {
            using (quadro)
            {
                byte[] bytesImagem = new byte[quadro.PixelDataLength];
                quadro.CopyPixelDataTo(bytesImagem);

                return BitmapSource.Create(quadro.Width, quadro.Height,960 , 960, PixelFormats.Bgr32, null, bytesImagem, quadro.Width * quadro.BytesPerPixel);
            }
        }

        public TColorFrame(ColorImageFrame sensorFrame)
        {
            ColorData = sensorFrame.GetRawPixelData();

            PixelDataLength = sensorFrame.PixelDataLength;
            BytesPerPixel = sensorFrame.BytesPerPixel;
            FrameNumber = sensorFrame.FrameNumber;
            Width = sensorFrame.Width;
            Height = sensorFrame.Height;
            Timestamp = sensorFrame.Timestamp;
        }

        public void Record(ColorImageFrame frame)
        {
            if (writer == null)
                throw new Exception("This recorder is stopped");

            if (colorRecoder == null)
                throw new Exception("Color recording is not actived on this KinectRecorder");

            colorRecoder.Record(frame);
            Flush();
        }

        private byte[] ObterImagemSensorRGB(ColorImageFrame quadro)
        {
            if (quadro == null) return null;

            using (quadro)
            {
                byte[] bytesImagem = new byte[quadro.PixelDataLength];
                quadro.CopyPixelDataTo(bytesImagem);

                return bytesImagem;
            }
        }

 private ImageSource ObterImagemRGB(ColorImageFrame quadro)
 {
     if (quadro == null) return null;
     using (quadro)
     {
         byte[] bytesImagem = new byte[quadro.PixelDataLength];
         quadro.CopyPixelDataTo(bytesImagem);
         return BitmapSource.Create(quadro.Width, quadro.Height,
         96, 96, PixelFormats.Gray16, null, bytesImagem,
         quadro.Width * quadro.BytesPerPixel);
     }  
 }

        public ReplayColorImageFrame(ColorImageFrame frame)
        {
            Format = frame.Format;
            BytesPerPixel = frame.BytesPerPixel;
            FrameNumber = frame.FrameNumber;
            TimeStamp = frame.Timestamp;
            Width = frame.Width;
            Height = frame.Height;

            PixelDataLength = frame.PixelDataLength;
            internalFrame = frame;
        }

 public void Update(ColorImageFrame frame)
 {
     var pixelData = new byte[frame.PixelDataLength];
     frame.CopyPixelDataTo(pixelData);
     if (Bitmap == null)
     {
         Bitmap = new WriteableBitmap(frame.Width, frame.Height,
         96, 96, PixelFormats.Bgr32, null);
     }
     int stride = Bitmap.PixelWidth * Bitmap.Format.BitsPerPixel / 8;
     Int32Rect dirtyRect = new Int32Rect(0, 0, Bitmap.PixelWidth, Bitmap.PixelHeight);
     Bitmap.WritePixels(dirtyRect, pixelData, stride, 0);
     RaisePropertyChanged(() => Bitmap);
 }

        public void synchronize(
            DepthImageFrame depthFrame,
            ColorImageFrame colorFrame,
            SkeletonFrame skletonFrame,
            Boolean isPauseMode
            )
        {
            IsPauseMode = isPauseMode;
            colorFrame.CopyPixelDataTo(_colorByte);

            //Console.WriteLine("max depth: "+depthFrame.MaxDepth);
            depthFrame.CopyDepthImagePixelDataTo(_depthPixels);

            _sensor.CoordinateMapper.MapColorFrameToDepthFrame(
                ColorImageFormat.RgbResolution640x480Fps30,
                DepthImageFormat.Resolution640x480Fps30,
                _depthPixels,
                _depthPoint
                );

            for (int i = 0; i < _pixelDepthDataLength; i++)
            {
                _depthShort[i] = (short)_depthPoint[i].Depth;
                _depthByte[i] = (byte)(_depthPoint[i].Depth*0.064-1);
            }

            skletonFrame.CopySkeletonDataTo(totalSkeleton);
            Skeleton firstSkeleton = (from trackskeleton in totalSkeleton
                                      where trackskeleton.TrackingState == SkeletonTrackingState.
                                      Tracked
                                      select trackskeleton).FirstOrDefault();

            _isCreation = true;
            if (firstSkeleton != null)
            {
                if (firstSkeleton.Joints[JointType.Spine].TrackingState == JointTrackingState.Tracked)
                {
                    IsSkeletonDetected = true;
                    UserSkeleton[SkeletonDataType.RIGHT_HAND] =
                        ScalePosition(firstSkeleton.Joints[JointType.HandRight].Position);
                    UserSkeleton[SkeletonDataType.LEFT_HAND] =
                        ScalePosition(firstSkeleton.Joints[JointType.HandLeft].Position);
                    UserSkeleton[SkeletonDataType.SPINE] =
                        ScalePosition(firstSkeleton.Joints[JointType.Spine].Position);
                    return;
                }
            }
            IsSkeletonDetected = false;
            _isCreation = false;
        }

 private BitmapSource ObterImagemSensorRGB(ColorImageFrame quadro)
 {
     using (quadro)
     {
         byte[] bytesImagem = new byte[quadro.PixelDataLength];
         quadro.CopyPixelDataTo(bytesImagem);
         int dpiX = 96;
         int dpiY = 96;
         int strideImage = quadro.Width * quadro.BytesPerPixel;
         return BitmapSource.Create(quadro.Width, quadro.Height,
         dpiX, dpiY, PixelFormats.Bgr32, null, bytesImagem,
         strideImage);
     }
 }