/// <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;
        }
Пример #9
0
 public bool Equals(KinectPoint kinectPoint)
 {
     if (this.X == kinectPoint.X &&
         this.Y == kinectPoint.Y &&
         this.Z == kinectPoint.Z)
     {
         return true;
     }
     else
     {
         return false;
     }
 }
Пример #10
0
 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;
        }