private void SensorAllFramesReady(object sender, AllFramesReadyEventArgs e) { Skeleton[] skeletons = new Skeleton[0]; bool depthReceived = false; _ultimoFrameTomado = DateTime.Now.Ticks; using (DepthImageFrame framesDistancia = e.OpenDepthImageFrame()) { if (framesDistancia == null) { return; } framesDistancia.CopyDepthImagePixelDataTo(this.depthPixels); depthReceived = true; if (datosDistancia == null) { datosDistancia = new short[framesDistancia.PixelDataLength]; } if (colorImagenDistancia == null) { colorImagenDistancia = new byte[framesDistancia.PixelDataLength * 4]; } framesDistancia.CopyPixelDataTo(datosDistancia); } using (ColorImageFrame colorFrame = e.OpenColorImageFrame()) { if (colorFrame != null) { colorFrame.CopyPixelDataTo(this.colorPixels); System.Drawing.Bitmap bmp = EmguCVHelper.ImageToBitmap(colorFrame); videoController.InicioGrabacion = DateTime.Now.Ticks; if (videoController.framesBmp.Count < 2000) { videoController.framesBmp.Add(bmp); } else { videoController.ForzarReinicioDeVideo(); } Image <Hsv, Byte> currentFrameHSV = new Image <Hsv, byte>(bmp); Image <Gray, Byte> grayFrame = currentFrameHSV.Convert <Gray, Byte>(); Image <Gray, Byte> imageHSVDest = currentFrameHSV.InRange(lowerLimit, upperLimit); imageHSVDest.Erode(100); VectorOfVectorOfPoint vectorOfPoint = EmguCVHelper.FindContours(imageHSVDest); for (int i = 0; i < vectorOfPoint.Size; i++) { var contour = vectorOfPoint[i]; var area = CvInvoke.ContourArea(contour); if (area > 100) { System.Drawing.Rectangle rec = CvInvoke.BoundingRectangle(contour); Point p1 = new Point(rec.X, rec.Y); Point p2 = new Point(rec.X + rec.Width, rec.Y + rec.Height); ObjetoX = (p1.X + p2.X) / 2; ObjetoY = (p1.Y + p2.Y) / 2; if (true == depthReceived) { this.sensor.CoordinateMapper.MapDepthFrameToColorFrame( DepthFormat, this.depthPixels, ColorFormat, this.colorCoordinates); int depthIndex = (int)ObjetoX + ((int)ObjetoY * this.depthWidth); DepthImagePixel depthPixel = this.depthPixels[depthIndex]; ObjetoZ = datosDistancia[depthIndex] >> 3; int X = (int)ObjetoX / this.colorToDepthDivisor; int Y = (int)ObjetoY / this.colorToDepthDivisor; } if (ObjetoZ > 0) { skelObjeto = DistanceHelper.ObtenerSkelPoint((int)ObjetoX, (int)ObjetoY, ObjetoZ, this.sensor); flagObjeto = true; } } } colorFrame.CopyPixelDataTo(this.colorPixels); // Write the pixel data into our bitmap this.colorBitmap.WritePixels( new Int32Rect(0, 0, this.colorBitmap.PixelWidth, this.colorBitmap.PixelHeight), this.colorPixels, this.colorBitmap.PixelWidth * sizeof(int), 0); } } using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame()) { if (skeletonFrame != null) { skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength]; skeletonFrame.CopySkeletonDataTo(skeletons); } } using (DrawingContext dc = this.drawingGroup.Open()) { // Draw a transparent background to set the render size //dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight)); dc.DrawImage(this.colorBitmap, new Rect(0.0, 0.0, RenderWidth, RenderHeight)); if (skeletons.Length != 0) { foreach (Skeleton skel in skeletons) { //Skeleton skel = skeletons[0]; if (skel.TrackingState == SkeletonTrackingState.Tracked) { this.DrawBonesAndJoints(skel, dc); } //Toma de mediciones de mano, hombro y codo derecho: ManoDerecha = skel.Joints[JointType.HandRight]; //Joint munecaDer = skel.Joints[JointType.WristRight]; CodoDerecho = skel.Joints[JointType.ElbowRight]; HombroDerecho = skel.Joints[JointType.ShoulderRight]; //Dibujo un punto negro sobre el objeto detectado Point objeto = new Point(this.ObjetoX, this.ObjetoY); dc.DrawEllipse(Brushes.Black, new Pen(Brushes.Black, 5), objeto, 5, 5); if ((HombroDerecho.TrackingState == JointTrackingState.Tracked) && (ManoDerecha.TrackingState == JointTrackingState.Tracked) && (CodoDerecho.TrackingState == JointTrackingState.Tracked)) { if (flagObjeto && !flagSkeleton) { puntos = new Puntos(HombroDerecho, CodoDerecho, ManoDerecha, skelObjeto); CalcularAngulosFinales(puntos); } //Console.WriteLine($"Mano X Y Z {handRight.Position.X} {handRight.Position.Y} {handRight.Position.Z}"); //Console.WriteLine($"Objeto X Y Z {skelObjeto.X} {skelObjeto.Y} {skelObjeto.Z}"); #region FinalizacionDelEjercicio if (!_objetoTomado && DistanceHelper.ObtenerDistancia(ManoDerecha, skelObjeto) < 0.2) { _objetoTomadoTiempo = DateTime.Now.AddSeconds(1).Ticks; _objetoTomado = true; } if (!_brazoMovido && _objetoTomadoTiempo < DateTime.Now.Ticks) { _vueltaPosicionInicialTiempo = DateTime.Now.AddSeconds(1).Ticks; arduinoController.EnviarAngulosFromAngulosServos(new AngulosServos(ArduinoController.BRAZO_GB)); _brazoMovido = true; } if (_vueltaPosicionInicialTiempo < DateTime.Now.Ticks) { Ejercicio.FinalizoConExito = true; Cerrar(); } #endregion FinalizacionDelEjercicio } } } // prevent drawing outside of our render area this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight)); } }