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