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