//Event Kinect New Frame void AllFramesReady(object sender, AllFramesReadyEventArgs e) { int index = Array.IndexOf(this.sensors, (KinectSensor)sender); KinectSensor sensor = (KinectSensor)sender; //Console.WriteLine("Sensor" + index + " fired."); // Grab frames from stream DepthImageFrame imageDepthFrame = e.OpenDepthImageFrame(); ColorImageFrame imageColorFrame = e.OpenColorImageFrame(); SkeletonFrame imageSkeletonFrame = e.OpenSkeletonFrame(); if (imageDepthFrame != null && (!colorOn || imageColorFrame != null) && imageSkeletonFrame != null && Semaphore.glControlLoaded) { CoordinateMapper mapper = new CoordinateMapper(sensor); SkeletonPoint[] skeletonPoints = new SkeletonPoint[imageDepthFrame.PixelDataLength]; DepthImagePixel[] depthPixels = new DepthImagePixel[imageDepthFrame.PixelDataLength]; // Copy the pixel data from the image to a temporary array imageDepthFrame.CopyDepthImagePixelDataTo(depthPixels); // Map Depth data to Skeleton points. // skeletonPoints is being changed as a result of this function call mapper.MapDepthFrameToSkeletonFrame(DEPTH_IMAGE_FORMAT, depthPixels, skeletonPoints); short[,] vertexData; if (colorOn) { //Allocate array for color data pixelColorData = new byte[sensor.ColorStream.FramePixelDataLength]; imageColorFrame.CopyPixelDataTo(pixelColorData); // Adjust coordinates of skeleton points according to the colour format // skeletonPoints is being changed as a result of this function call mapper.MapColorFrameToSkeletonFrame(COLOR_IMAGE_FORMAT, DEPTH_IMAGE_FORMAT, depthPixels, skeletonPoints); vertexData = new short[(frameHeight * frameWidth), 6]; } else { vertexData = new short[(frameHeight * frameWidth), 3]; } // Convert SkeletonPoints data into short[][] // [x, y, z, Blue, Green, Red] int i = 0; for (int row = 0; row < frameHeight * frameWidth; row++) { vertexData[row, 0] = (short)(skeletonPoints[row].X * 1000); //Store for X vertexData[row, 1] = (short)(skeletonPoints[row].Y * 1000); //Store for Y vertexData[row, 2] = (short)(skeletonPoints[row].Z * 1000); //Store for Z if (colorOn) { vertexData[row, 3] = (short)pixelColorData[i + 2]; vertexData[row, 4] = (short)pixelColorData[i + 1]; vertexData[row, 5] = (short)pixelColorData[i]; i += 4; } } // Pass data to write to file if (Semaphore.readyForPCD)//change to use semaphore { Semaphore.passPCD(vertexData, index); } // Dispose frames for memory imageDepthFrame.Dispose(); if (colorOn) { imageColorFrame.Dispose(); } imageSkeletonFrame.Dispose(); } }
public void FindBottle(object sender,AllFramesReadyEventArgs e) { //contour Hu矩不靠谱!!!! //if (isBottleFound == false) //{ // SkeletonPoint bottleCoordinate; // var colorframe = sensor.ColorStream.OpenNextFrame(100); // var colorimage = colorframe.ToOpenCVImage<Bgr, Byte>(); // var depthframe = sensor.DepthStream.OpenNextFrame(100); // //var depthimage = depthframe.ToOpenCVImage<Gray, Byte>(); // using (var depthimage = depthframe.ToOpenCVImage<Gray, Byte>()) // { // double recognitionResult = 0; // Image<Gray, Byte> imageThreshold = depthimage.ThresholdBinary(new Gray(100d), new Gray(255d)); // Image<Bgr, Byte> imageBottle = new Image<Bgr, byte>("spirit.jpg"); // Image<Gray, Byte> imageBottleGray = imageBottle.Convert<Gray, Byte>(); // Image<Gray, Byte> imageBottleThreshold = imageBottleGray.ThresholdBinary(new Gray(100d), new Gray(255d)); // Contour<Drawing.Point> contour1 = imageThreshold.FindContours(CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_SIMPLE, RETR_TYPE.CV_RETR_EXTERNAL); // Contour<Drawing.Point> contour2 = imageBottleThreshold.FindContours(CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_SIMPLE, RETR_TYPE.CV_RETR_EXTERNAL); // recognitionResult = MatchShapes(contour1, contour2); // if (recognitionResult < 10) //maybe!!!!!!!! // { //// bottleCoordinate = DetermineCoordinate(contour1); //// } //// MoveRobotToDestination(); // } //} if (isBottleFound == false) { DepthImageFrame depthframe = e.OpenDepthImageFrame(); ColorImageFrame colorframe = e.OpenColorImageFrame(); SkeletonPoint bottleCoordinate; ColorImagePoint[] colorpoint = new ColorImagePoint[sensor.ColorStream.FramePixelDataLength]; //ColorImagePoint colorImagePoint = new ColorImagePoint(); DepthImagePoint depthImagePoint = new DepthImagePoint(); DepthImagePoint[] depthPoints = new DepthImagePoint[sensor.DepthStream.FramePixelDataLength]; //DepthImagePoint[] depthPoint = new DepthImagePoint[this.sensor.DepthStream.FramePixelDataLength]; //SkeletonPoint[] skeletonPoint = new SkeletonPoint[this.sensor.SkeletonStream.FrameSkeletonArrayLength]; SkeletonPoint[] skeletonPoint = new SkeletonPoint[this.sensor.DepthStream.FramePixelDataLength]; //var depthFrame = e.OpenDepthImageFrame(); // var colorframe = e.OpenColorImageFrame(); this.depthPixels = new DepthImagePixel[this.sensor.DepthStream.FramePixelDataLength]; if(depthframe !=null && colorframe != null) { depthframe.CopyDepthImagePixelDataTo(this.depthPixels); var colorimage = colorframe.ToOpenCVImage<Bgr, Byte>(); Drawing.Point middlePoint = new Drawing.Point(0, 0); //var depthframe = sensor.DepthStream.OpenNextFrame(100); long matchTime; MKeyPoint[] keyPoints; double number; List<int> index; using (Image<Gray, Byte> modelImage = new Image<Gray, byte>("spirit.png")) using (Image<Gray, Byte> observedImage = colorframe.ToOpenCVImage<Gray, Byte>()) { Image<Bgr, byte> result = DrawMatches.Draw(modelImage, observedImage, out matchTime, out number, out keyPoints, out index); resultImage.Source = result.ToBitmapSource(); } if (number >= 6) { //MoveRobotToDestination(); //假设 sensor.AllFramesReady -= FindBottle; #region GetbottleCoordinate CoordinateMapper changeCoordinate = new CoordinateMapper(sensorChooser.Kinect); //changeCoordinate.MapColorFrameToSkeletonFrame(ColorImageFormat.RgbResolution640x480Fps30, DepthImageFormat.Resolution640x480Fps30 // ,depthPixels, skeletonPoint); // changeCoordinate.MapColorFrameToDepthFrame(ColorImageFormat.RgbResolution1280x960Fps12, DepthImageFormat.Resolution640x480Fps30, depthPixels,depthPoint); foreach (var _index in index) { middlePoint.X += (int)keyPoints[_index].Point.X; middlePoint.Y += (int)keyPoints[_index].Point.Y; } middlePoint.X = middlePoint.X / index.Count; middlePoint.Y = middlePoint.Y / index.Count; //暂定如此!!!! //changeCoordinate.MapColorFrameToDepthFrame(ColorImageFormat.RgbResolution640x480Fps30, DepthImageFormat.Resolution640x480Fps30, depthPixels, depthPoints); changeCoordinate.MapColorFrameToSkeletonFrame(ColorImageFormat.RgbResolution640x480Fps30, DepthImageFormat.Resolution640x480Fps30, depthPixels,skeletonPoint); //depthImagePoint.X = middlePoint.X; //depthImagePoint.Y = middlePoint.Y; //depthImagePoint.Depth = depthPoints[depthImagePoint.Y * 640 + depthImagePoint.X].Depth; //bottleCoordinate = changeCoordinate.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, depthImagePoint); #endregion GetbottleCoordinate long Skeletonindex = 640 * middlePoint.Y + middlePoint.X; bottleCoordinate = skeletonPoint[Skeletonindex]; //bottleCoordinate.X = 10; //bottleCoordinate.Y = 10; coordinate.Text = string.Format("{0:F4},{1:F4},{2:F4}", bottleCoordinate.X, bottleCoordinate.Y, bottleCoordinate.Z); MoveRobotToDestination(); } } } }