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