/// <summary> /// This method is used to convert an array of KinectPoints to a ByteArray.</summary> /// <param name="kinArray">the array that should be converted</param> /// <param name="width">the width of the passed two-dimensional array, e.g. 640</param> /// <param name="height">the height of the passed two-dimensional array, e.g. 480</param> /// <returns>Returns the passed array as a one-dimensional array of bytes</returns> public byte[] ConvertKinectPointArrayToByteArray(KinectPoint[,] kinArray, int width, int height) { var stride = width * Kinect.BYTES_PER_PIXEL; byte[] pixelData = new byte[height * stride]; int index = 0; for (int y = 0; y < height; ++y) { for (int x = 0; x < width; ++x) { if (kinArray[x, y].Z != -1) { pixelData[index + 2] = (byte)kinArray[x, y].R; pixelData[index + 1] = (byte)kinArray[x, y].G; pixelData[index] = (byte)kinArray[x, y].B; } else { pixelData[index + 2] = 0xFF; pixelData[index + 1] = 0x00; pixelData[index] = 0x00; } index += Kinect.BYTES_PER_PIXEL; } } return pixelData; }
/// <summary> /// This method is used to calculate the RealWorldCalculation-coordinates for a given KinectPoint /// When to use: use only to calculate the corner points' RealWorldCalculation-coordinates. For other KinectPoints use the other "CalculateRealWorldVector" /// method. /// Important: The passed KinectPoint must not be NULL! /// </summary> /// <param name="p">the KinectPoint which should be transformed to a RealWorldPoint </param> /// <returns>Returns the RealWorldPoint corresponding to the passed KinectPoint</returns> private RealWorldPoint CalculateRealWorldPoint(KinectPoint p) { var rwPoint = new RealWorldPoint(); rwPoint.Z = p.Z; rwPoint.X = (int) ((p.X - (Kinect.KINECT_IMAGE_WIDTH / 2))* rwPoint.Z/Kinect.WIDTH_CONST); rwPoint.Y = (int)((p.Y - (Kinect.KINECT_IMAGE_HEIGHT / 2)) * rwPoint.Z /Kinect.HEIGHT_CONST); return rwPoint; }
public static List<KinectPoint> ExtractBlackPoints(KinectPoint[,] kinectPoints) { var blackPixel = new List<KinectPoint>(); for (var i = 0; i < 480; i++) { for (var j = 0; j < 640; j++) { var p = kinectPoints[j, i]; if (p.R - BLACK_TRESHOLD < 0 && p.G - BLACK_TRESHOLD < 0 && p.B - BLACK_TRESHOLD < 0) { blackPixel.Add(new KinectPoint{X = j, Y = i}); } } } return blackPixel; }
public static List<Vector2D> ExtractBlackPointsAs2dVector(KinectPoint[,] kinectPoints) { var blackPixel = new List<Vector2D>(); for (var i = 0; i < 480; i++) { for (var j = 0; j < 640; j++) { KinectPoint p = kinectPoints[j, i]; if (p.R - BLACK_TRESHOLD < 0 && p.G - BLACK_TRESHOLD < 0 && p.B - BLACK_TRESHOLD < 0) { blackPixel.Add(Get2dVectorFromKinectPoint(p)); } } } return blackPixel; }
/// <summary> /// This method is used to convert an array of KinectPoints to a Bitmap.</summary> /// <param name="kinArray">the array that should be converted</param> /// <param name="width">the width of the passed two-dimensional array, e.g. 640</param> /// <param name="height">the height of the passed two-dimensional array, e.g. 480</param> /// <returns>Returns the passed array as a bitmap ready to use for drawing</returns> public Bitmap ConvertKinectPointArrayToBitmap(KinectPoint[,] kinArray, int width, int height) { Bitmap bmp = new Bitmap(width, height); for (int y = 0; y < height; ++y) { for (int x = 0; x < width; ++x) { KinectPoint p = kinArray[x, y]; if (p.Z != -1) { System.Drawing.Color c = System.Drawing.Color.FromArgb(0, p.R, p.G, p.B); bmp.SetPixel(p.X, p.Y, c); } } } return bmp; }
/// <summary> /// This method converts a byte array into an array of KinectPoints</summary> /// <param name="bytearray">the array which should be converted</param> /// <param name="width">the width of the new two-dimensional array, e.g. 640</param> /// <param name="height">the height of the new two-dimensional array, e.g. 480</param> /// <returns>Returns the converted byte array as an array of KinectPoints</returns> public KinectPoint[,] ConvertByteArrayToKinectPoint(byte[] bytearray, int width, int height) { int bitPositionByteArray = 0; KinectPoint[,] kinectArray = new KinectPoint[width, height]; for (int y = 0; y < height; ++y) { for (int x = (width - 1); x >= 0; --x) { kinectArray[x, y] = new KinectPoint( x, y, (byte)bytearray[bitPositionByteArray + 2], (byte)bytearray[bitPositionByteArray + 1], (byte)bytearray[bitPositionByteArray]); bitPositionByteArray += Kinect.BYTES_PER_PIXEL; } } return kinectArray; }
/// <summary> /// This method is used to recover missing depth information of the KinectPoints delivered by the Kinect. /// This solves the problem of having incorrect depth values or NULL-values in the KinectPoint-array. /// Important: The array must not contain only NULL-Points! /// </summary> /// <param name="kinArray">the array whose KinectPoint-values have to be corrected.</param> /// <returns>Returns a new KinectPoint-array containing valid depth information for every KinectPoint</returns> public KinectPoint[,] RecoverDepthInformationOfKinectPointArray(KinectPoint[,] kinArray) { var recoveredKinArray = new KinectPoint[Kinect.KINECT_IMAGE_WIDTH,Kinect.KINECT_IMAGE_HEIGHT]; for (int y = 0; y < Kinect.KINECT_IMAGE_HEIGHT; ++y) { for (int x = 0; x < Kinect.KINECT_IMAGE_WIDTH; ++x) { if (kinArray[x,y].Z == -1) { int correctionRadius = 1; //int z = -1; int depthMeanValue = 0; do { var neighbors = GetNeighborsOfKinectPoint(kinArray, kinArray[x,y], correctionRadius); depthMeanValue = CalculateDepthMeanValue(neighbors); ++correctionRadius; } while (depthMeanValue == 0); kinArray[x,y].Z = depthMeanValue; recoveredKinArray[x,y] = kinArray[x,y]; } else { recoveredKinArray[x, y] = kinArray[x, y]; } } } return recoveredKinArray; }
/// <summary> /// This method is used to calculate the RealWorldCalculation-coordinates for a given KinectPoint /// When to use: use only to calculate the corner points' RealWorldCalculation-coordinates. For other KinectPoints use the other "CalculateRealWorldVector" /// method. /// How to use: The parameters rwA, rwB and rwC represent the corner points of the detected field. rwB is the RealWorld-point which is a direct /// of the other RealWorld-points rwA and rwC. Direct means, that those point can be reached from rwB by following the adjacent outlines of the field. /// Important: The passed KinectPoint must not be NULL! /// </summary> /// <param name="p">the KinectPoint which should be transformed to a RealWorldPoint </param> /// <param name="rA">the RealWorldPoint A which represents a corner point and /// is used to calculate the coordinates for the new RealWorldPoints </param> /// <param name="rB">the RealWorldPoint B which represents a corner point and /// is used to calculate the coordinates for the new RealWorldPoints </param> /// <param name="rC">the RealWorldPoint C which represents a corner point and /// is used to calculate the coordinates for the new RealWorldPoints </param> /// <returns>Returns the RealWorldPoint corresponding to the passed KinectPoint</returns> private RealWorldPoint CalculateRealWorldPoint(KinectPoint p, RealWorldPoint rA, RealWorldPoint rB, RealWorldPoint rC) { var rwPoint = new RealWorldPoint(); var vec_rA = CreateRealWorldVector(rA); var vec_rB = CreateRealWorldVector(rB); var vec_rC = CreateRealWorldVector(rC); var vec_N = (vec_rA.Subtract(vec_rB)).CrossProduct(vec_rC.Subtract(vec_rB)); var q = vec_N.ScalarProduct(vec_rB); var x = (int)((p.X - (Kinect.KINECT_IMAGE_WIDTH / 2)) /Kinect.WIDTH_CONST); var y = (int)((p.Y - (Kinect.KINECT_IMAGE_HEIGHT / 2)) /Kinect.HEIGHT_CONST); var vec_rP = new Vector3D(x,y,1); rwPoint.Z = (int)(q / (vec_N.ScalarProduct(vec_rP))); rwPoint.X = (int) ((p.X - (Kinect.KINECT_IMAGE_WIDTH / 2))*rwPoint.Z/Kinect.WIDTH_CONST); rwPoint.Y = (int)((p.Y - (Kinect.KINECT_IMAGE_HEIGHT / 2)) * rwPoint.Z /Kinect.HEIGHT_CONST); return rwPoint; }
public bool Equals(KinectPoint kinectPoint) { if (this.X == kinectPoint.X && this.Y == kinectPoint.Y && this.Z == kinectPoint.Z) { return true; } else { return false; } }
private static Vector3D Get3dVectorFromKinectPoint(KinectPoint p) { return new Vector3D { X = p.X, Y = p.Y, Z = p.Z}; }
/// <summary> /// /// </summary> /// <param name="newPicKin"></param> /// <param name="width"></param> /// <param name="height"></param> /// <returns></returns> public byte[] PrintKinectPointArray(KinectPoint[,] newPicKin, int width, int height) { var stride = width * 4; // bytes per row byte[] pixelData = new byte[height * stride]; int index = 0; for (int y = 0; y < height; ++y) { for (int x = 0; x < width; ++x) { if (newPicKin[x, y] != null) { pixelData[index + 2] = (byte)newPicKin[x, y].R; pixelData[index + 1] = (byte)newPicKin[x, y].G; pixelData[index] = (byte)newPicKin[x, y].B; } else { pixelData[index + 2] = 0xFF; pixelData[index + 1] = 0x00; pixelData[index] = 0x00; } index += 4; } } return pixelData; //wrBitmap.WritePixels(this._colorImageBitmapRect, pixelData, this._colorImageStride, 0); }
/// <summary> /// This method is used to reflect the picture seen by the kinect on the y-axis. /// So for example the Point (0,0) becomes the Point (639,0). /// The y-values do not change. </summary> /// <param name="kinArray">the array which should be transformed</param> /// <param name="width">the width of the new two-dimensional array, e.g. 640</param> /// <param name="height">the height of the new two-dimensional array, e.g. 480</param> /// <returns>Returns the flipped KinectPoint-Array</returns> public KinectPoint[,] FlipArray(KinectPoint[,] kinArray, int width, int height) { var newKinArray = new KinectPoint[width,height]; for (int y = 0; y < height; ++y) { for (int x = (width - 1); x >= 0; --x) { newKinArray[x, y] = kinArray[((width - 1) - x), y]; } } return newKinArray; }
/// <summary> /// This method is used to convert an array of KinectPoints to a WriteableBitmap.</summary> /// <param name="kinArray">the array that should be converted</param> /// <param name="width">the width of the passed two-dimensional array, e.g. 640</param> /// <param name="height">the height of the passed two-dimensional array, e.g. 480</param> /// <returns>Returns the passed array written to a Bitmap ready to use it in a WPF- or WinForms-Project</returns> public WriteableBitmap ConvertKinectPointArrayToWritableBitmap(KinectPoint[,] kinArray, int width, int height) { var stride = width * Kinect.BYTES_PER_PIXEL; byte[] pixelData = new byte[height * stride]; int index = 0; for (int y = 0; y < height; ++y) { for (int x = 0; x < width; ++x) { if (kinArray[x, y].Z != -1) { pixelData[index + 2] = (byte)kinArray[x, y].R; pixelData[index + 1] = (byte)kinArray[x, y].G; pixelData[index] = (byte)kinArray[x, y].B; } else { pixelData[index + 2] = 0xFF; pixelData[index + 1] = 0x00; pixelData[index] = 0x00; } index += Kinect.BYTES_PER_PIXEL; } } _kinect._colorImageBitmap.WritePixels(_kinect._colorImageBitmapRect, pixelData, _kinect._colorImageStride, 0); return _kinect._colorImageBitmap; }
public byte[] CompareZCalcStrategies(IKinectToRealWorldStrategy kinectToRealWorldStrategy) { var kinectPoints = kinect.CreateKinectPointArray(); var kinectPointsList = new List<KinectPoint>(); for (int y = 0; y < Kinect.Kinect.KINECT_IMAGE_HEIGHT; ++y) { for (int x = 0; x < Kinect.Kinect.KINECT_IMAGE_WIDTH; ++x) kinectPointsList.Add(kinectPoints[x, y]); } var dictPoints = kinectToRealWorldStrategy.TransformKinectToRealWorld(kinect, kinectPointsList); var diffPoints = new KinectPoint[Kinect.Kinect.KINECT_IMAGE_WIDTH, Kinect.Kinect.KINECT_IMAGE_HEIGHT]; var differences = new List<int>(); foreach (var p in dictPoints) { differences.Add(p.Key.Z - p.Value.Z); } differences.Sort(); var maxDiff = differences.Max(); var minDiff = differences.Min(); var rangeDiff = maxDiff - minDiff; var r = 0x80; var g = 0x80; var b = 0x80; foreach (var p in dictPoints) { int diff = p.Key.Z - p.Value.Z; //diff positiv --> gegen weiss, diff negativ --> gegen schwarz if (diff == 0) { r = 0x00; b = 0x00; g = 0xFF; } else { r = 0x80 + (int)(diff * 127.0 / rangeDiff); g = 0x80 + (int)(diff * 127.0 / rangeDiff); b = 0x80 + (int)(diff * 127.0 / rangeDiff); } diffPoints[p.Key.X, p.Key.Y] = new KinectPoint(p.Key.X, p.Key.Y, diff, r, g, b); } var edgepoints = Calibration.GetEdgePoints(); foreach (var edgepoint in edgepoints) { var x = edgepoint.KinectPoint.X; var y = edgepoint.KinectPoint.Y; diffPoints[x, y] = new KinectPoint(x, y, 0x00, 0x00, 0xFF); } return kinect.ConvertKinectPointArrayToByteArray(diffPoints, Kinect.Kinect.KINECT_IMAGE_WIDTH, Kinect.Kinect.KINECT_IMAGE_HEIGHT); }
private KinectPoint[,] CalculateKMeans(KinectPoint[,] newPic) { try { List<Vector2D> centroids = KMeans.DoKMeans(KMeansHelper.ExtractBlackPointsAs2dVector(newPic), KMeansHelper.CreateInitialCentroids(640, 480)); foreach (var vectorCentroid in centroids) { newPic[(int) vectorCentroid.X, (int) vectorCentroid.Y].R = 255; newPic[(int) vectorCentroid.X, (int) vectorCentroid.Y].G = 0; newPic[(int) vectorCentroid.X, (int) vectorCentroid.Y].B = 0; //bitmap.SetPixel((int)vectorCentroid.X, (int)vectorCentroid.Y, Color.Red); MessageBox.Show("X: " + vectorCentroid.X.ToString() + ", Y: " + vectorCentroid.Y.ToString()); } } catch (Exception e) { MessageBox.Show(e.StackTrace); } return newPic; }
private static Vector2D Get2dVectorFromKinectPoint(KinectPoint p) { return new Vector2D { X = p.X, Y = p.Y }; }
/// <summary> /// This method searches all neighbors of a KinectPoint within a KinectPoint-array /// Important: The array must not contain only NULL-Points! /// </summary> /// <param name="kinArray">the array which contains the needed KinectPoints </param> /// /// <param name="kinPoint">the KinectPoint whose neighbors should be found </param> /// /// <param name="correctionRadius">the radius specifies how far away KinectPoints can be from the kinPoint to be considered as neighbor, /// e.g. correctionRadius = 1 means that only direct neighbors of the kinPoint are found.</param> /// <returns>Returns a list containing all neighbors of the specified kinPoint within the correctionRadius</returns> private List<KinectPoint> GetNeighborsOfKinectPoint(KinectPoint[,] kinArray, KinectPoint kinPoint, int correctionRadius) { var neighbors = new List<KinectPoint>(); for (int i = correctionRadius; i >= -correctionRadius; --i) { for (int j = correctionRadius; j >= -correctionRadius; --j) { int x = kinPoint.X + i; int y = kinPoint.Y + j; if ((x >= 0 && x < Kinect.KINECT_IMAGE_WIDTH) && (y >= 0 && y < Kinect.KINECT_IMAGE_HEIGHT)) { neighbors.Add(kinArray[x, y]); } } } return neighbors; }