void sensor_AllFramesReady(object sender, Microsoft.Kinect.AllFramesReadyEventArgs e) { if (this.command == "Stop") { Bot.stop(); } if (this.command == "Forward") { Bot.traverse(); } if (this.command == "Right") { Bot.turnRight(); } if (this.command == "Left") { Bot.turnRight(); } xf++; if (xf % 5 == 0) { xf = 0; if (this.command != null) { using (ColorImageFrame colorFrame = e.OpenColorImageFrame()) { using (DepthImageFrame depthFrame = e.OpenDepthImageFrame()) { using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame()) { humanPosition = frameToHuman(skeletonFrame); if (colorFrame != null) { // Copy the pixel data from the image to a temporary array 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); // Error here due to OpenCV_core290.dll //int[] objPos = new int[2]; //objPos = tmp.matchColor(ImageProc.colorFrameToImage(colorFrame)); //if (objPos != null) //{ // short blobDepth = getDepthAtPoint(objPos, depthFrame); // this.lblObject.Content = objPos[0] + ", " + objPos[1] + ", " + blobDepth; //} //else //{ // this.lblObject.Content = "Null"; //} System.Drawing.Bitmap bmp = ImageProc.colorFrameToAforge(colorFrame); HSLFiltering filter = new HSLFiltering(); // set color ranges to keep if (objec[0] == -1) { if (command == "Fetching Bottle") { filter.Hue = bottleH; filter.Saturation = bottleS; filter.Luminance = bottleL; } else if (command == "Fetching Box") { filter.Hue = boxH; filter.Saturation = boxS; filter.Luminance = boxL; } //// apply the filter filter.ApplyInPlace(bmp); BlobCounter blobCounter = new BlobCounter(bmp); int i = blobCounter.ObjectsCount; ExtractBiggestBlob fil = new ExtractBiggestBlob(); int[] pp = new int[2]; pp[0] = 0; pp[1] = 0; int h = 0; if (i > 0) { fil.Apply(bmp); pp[0] = fil.BlobPosition.X; pp[1] = fil.BlobPosition.Y; h = fil.Apply(bmp).Height; } short blobDepth = getDepthAtPoint(pp, depthFrame); this.lblObject.Content = pp[0] + ", " + pp[1] + ", " + blobDepth; this.objec[0] = pp[0]; this.objec[1] = pp[1]; this.objec[2] = blobDepth; } else { filter.Hue = botH; filter.Saturation = botS; filter.Luminance = botL; filter.ApplyInPlace(bmp); BlobCounter blobCounter = new BlobCounter(bmp); int i = blobCounter.ObjectsCount; ExtractBiggestBlob fil = new ExtractBiggestBlob(); int[] pp = new int[2]; pp[0] = 0; pp[1] = 0; int h = 0; if (i > 0) { fil.Apply(bmp); pp[0] = fil.BlobPosition.X; pp[1] = fil.BlobPosition.Y; h = fil.Apply(bmp).Height; } short blobDepth = getDepthAtPoint(pp, depthFrame); this.lblBot.Content = pp[0] + ", " + pp[1] + ", " + blobDepth; this.bot[0] = pp[0]; this.bot[1] = pp[1]; this.bot[2] = blobDepth; } //Assign Manual Position to bot and object } if (humanPosition != null) { this.lblHuman.Content = humanPosition[0] + ", " + humanPosition[1] + ", " + humanPosition[2]; } else { this.lblHuman.Content = "No Human detected"; } if (this.path == 0) { if (humanPosition != null) { if (Bot.moveDoraemon(this.bot[0], this.humanPosition[0], this.bot[2], this.humanPosition[2]) == 0) { this.path = 1; } } } else { if (Bot.moveDoraemon(this.bot[0], this.objec[0], this.bot[2], this.objec[2]) == 0) { Bot.stop(); } } } } } this.lbl.Content = command; } } }
private void OnAllFramesReady(object sender, Microsoft.Kinect.AllFramesReadyEventArgs allFramesReadyEventArgs) { ColorImageFrame colorImageFrame = null; DepthImageFrame depthImageFrame = null; SkeletonFrame skeletonFrame = null; try { colorImageFrame = allFramesReadyEventArgs.OpenColorImageFrame(); depthImageFrame = allFramesReadyEventArgs.OpenDepthImageFrame(); skeletonFrame = allFramesReadyEventArgs.OpenSkeletonFrame(); if (colorImageFrame == null || depthImageFrame == null || skeletonFrame == null) { return; } // Check for image format changes. The FaceTracker doesn't // deal with that so we need to reset. if (this.depthImageFormat != depthImageFrame.Format) { this.depthImage = null; this.depthImageFormat = depthImageFrame.Format; } if (this.colorImageFormat != colorImageFrame.Format) { this.colorImage = null; this.colorImageFormat = colorImageFrame.Format; } // Create any buffers to store copies of the data we work with if (this.depthImage == null) { this.depthImage = new short[depthImageFrame.PixelDataLength]; } if (this.colorImage == null) { this.colorImage = new byte[colorImageFrame.PixelDataLength]; } // Get the skeleton information if (this.SkeletonData == null || this.SkeletonData.Length != skeletonFrame.SkeletonArrayLength) { this.SkeletonData = new Skeleton[skeletonFrame.SkeletonArrayLength]; } colorImageFrame.CopyPixelDataTo(this.colorImage); depthImageFrame.CopyPixelDataTo(this.depthImage); skeletonFrame.CopySkeletonDataTo(this.SkeletonData); Skeleton activeSkeleton = null; activeSkeleton = (from skel in this.SkeletonData where skel.TrackingState == SkeletonTrackingState.Tracked select skel).FirstOrDefault(); //Idea: Separate Eye-Parts of Color Image //Use learning Algorithm for right and left eye //Detect blink on separated parts of color Image //colorImage is one dimensional array with 640 x 480 x 4 (RGBA) values if (activeSkeleton != null) { FaceTrackFrame currentFaceFrame = faceTracker.Track(ColorImageFormat.RgbResolution640x480Fps30, colorImage, depthImageFormat, depthImage, activeSkeleton); float browRaiserValue = currentFaceFrame.GetAnimationUnitCoefficients()[AnimationUnit.BrowRaiser]; float browLowererValue = currentFaceFrame.GetAnimationUnitCoefficients()[AnimationUnit.BrowLower]; tbBrowLowerer.Text = browLowererValue.ToString(); tbBrowRaiser.Text = browRaiserValue.ToString(); //Get relevant Points for blink detection //Left eye int minX = (int)Math.Round(currentFaceFrame.GetProjected3DShape()[FeaturePoint.AboveOneFourthLeftEyelid].X); int minY = (int)Math.Round(currentFaceFrame.GetProjected3DShape()[FeaturePoint.AboveOneFourthLeftEyelid].Y); int maxX = (int)Math.Round(currentFaceFrame.GetProjected3DShape()[FeaturePoint.BelowThreeFourthLeftEyelid].X); int maxY = (int)Math.Round(currentFaceFrame.GetProjected3DShape()[FeaturePoint.BelowThreeFourthLeftEyelid].Y); Bitmap leftEye = EyeExtract(colorImageFrame, currentFaceFrame, minX, minY, maxX, maxY, false); pbLeftEye.Image = leftEye; //Right eye minX = (int)Math.Round(currentFaceFrame.GetProjected3DShape()[FeaturePoint.AboveThreeFourthRightEyelid].X); minY = (int)Math.Round(currentFaceFrame.GetProjected3DShape()[FeaturePoint.AboveThreeFourthRightEyelid].Y); maxX = (int)Math.Round(currentFaceFrame.GetProjected3DShape()[FeaturePoint.OneFourthBottomRightEyelid].X); maxY = (int)Math.Round(currentFaceFrame.GetProjected3DShape()[FeaturePoint.OneFourthBottomRightEyelid].Y); Bitmap rightEye = EyeExtract(colorImageFrame, currentFaceFrame, minX, minY, maxX, maxY, true); pbRightEye.Image = rightEye; //Wende Kantenfilter auf die beiden Augen an. double dxRight; double dyRight; double dxLeft; double dyLeft; if (rightEye != null && leftEye != null) { Bitmap edgePicRight = Convolution(ConvertGrey(rightEye), true, out dxRight, out dyRight); Bitmap edgePicLeft = Convolution(ConvertGrey(leftEye), false, out dxLeft, out dyLeft); //If Face is rotated, move Mouse if (headRotationHistory.Count > filterLength && currentFaceFrame.TrackSuccessful) { int x = 0; int y = 0; //Method 1: Ohne Glättung //ScaleXY(currentFaceFrame.Rotation, out x, out y); //MouseControl.Move(x, y); ////Method 2: Glättung über die letzten x Bilder: //int i = 0; //Vector3DF rotationMedium = new Vector3DF(); //while (i < 10 && headRotationHistory.Count - 1 > i) //{ // i++; // rotationMedium.X += headRotationHistory[headRotationHistory.Count - 1 - i].X; // rotationMedium.Y += headRotationHistory[headRotationHistory.Count - 1 - i].Y; //} //rotationMedium.X = rotationMedium.X / i; //rotationMedium.Y = rotationMedium.Y / i; //ScaleXY(rotationMedium, out x, out y); //MouseControl.Move(x, y); //Method 3: Gauß-Filter: Gewichte die letzten Bilder stärker. Vector3DF rotationMedium = new Vector3DF(); rotationMedium.X = currentFaceFrame.Rotation.X * gaussFilter[0]; rotationMedium.Y = currentFaceFrame.Rotation.Y * gaussFilter[0]; int i = 0; while (i < filterLength - 1) { i++; rotationMedium.X += (headRotationHistory[headRotationHistory.Count - 1 - i].X * gaussFilter[i]); rotationMedium.Y += (headRotationHistory[headRotationHistory.Count - 1 - i].Y * gaussFilter[i]); } rotationMedium.X = (float)(rotationMedium.X / gaussFactor); rotationMedium.Y = (float)(rotationMedium.Y / gaussFactor); ScaleXY(rotationMedium, out x, out y); MouseControl.Move(x, y); //Method 4: Quadratische Glättung //double deltaX = ((-currentFaceFrame.Rotation.Y) - (-headRotationHistory.Last().Y)); //double deltaY = ((-currentFaceFrame.Rotation.X) - (-headRotationHistory.Last().X)); //if (deltaX < 0) // deltaX = -Math.Pow(deltaX, 2) * 4; //else // deltaX = Math.Pow(deltaX, 2) * 4; //if (deltaY < 0) // deltaY = -Math.Pow(deltaY, 2) * 5; //else // deltaY = Math.Pow(deltaY, 2) * 5; //MouseControl.DeltaMove((int)Math.Round(deltaX, 0), (int)Math.Round(deltaY)); } headRotationHistory.Add(currentFaceFrame.Rotation); if (headRotationHistory.Count >= 100) { headRotationHistory.RemoveAt(0); } } } } catch (Exception e) { } finally { if (colorImageFrame != null) { colorImageFrame.Dispose(); } if (depthImageFrame != null) { depthImageFrame.Dispose(); } if (skeletonFrame != null) { skeletonFrame.Dispose(); } } }