private static SkeletonPoint GetFinger(SkeletonPoint Hand, SkeletonPoint Wrist, SkeletonPoint Elbow, int SkeletonID)
{
DepthImagePoint HandD = myKin.MapSkeletonPointToDepth(Hand, myKin.DepthStream.Format);
DepthImagePoint WristD = myKin.MapSkeletonPointToDepth(Wrist, myKin.DepthStream.Format);
//DepthImagePoint ElbowD = myKin.MapSkeletonPointToDepth(ElbowS, myKin.DepthStream.Format);
//double HandToWrist = Distance(HandS, WristS);
double WristToElbow = Distance(Wrist, Elbow);
double maxDistance = WristToElbow;
SkeletonPoint max = Hand;
XYQueue queue = new XYQueue();
queue.insert(WristD.X, WristD.Y);
queue.insert(HandD.X, HandD.Y);
do
{
int x, y;
queue.delete(out x, out y);
if (x >= 0 && x < myKin.DepthStream.FrameWidth && y >= 0 && y < myKin.DepthStream.FrameHeight) //safheye asli
{
if (GetPersonID(x, y, MapToCamera.Depth) == SkeletonID) //rooye adam
{
SkeletonPoint point = myKin.MapDepthToSkeletonPoint(myKin.DepthStream.Format, x, y, DepthMap[y * myKin.DepthStream.FrameWidth + x]);
if (Distance(Hand, point) < 0.1) //tooye mohite dast kafe dast
{
double b = Distance(Elbow, point);
if (b >= WristToElbow) //samte angosht
{
if (b > maxDistance) //doortarin angosht
{
maxDistance = b;
max = point;
}
queue.insert(x + 1, y);
queue.insert(x - 1, y);
queue.insert(x, y + 1);
queue.insert(x, y - 1);
}
}
}
}
} while (queue.Count != 0);
return(max);
}
public static Bitmap CameraStereoImage(int scaleDepth)
{
Bitmap result = null;
if (myKin != null)
{
Console.WriteLine("Camera Stereo Image:");
EnableColorStream();
EnableDepthStream();
if (!colorNeeded)
{
colorNeeded = true;
}
while (ColorMap == null)
{
;
}
while (DepthMappedToColor == null)
{
;
}
byte[] colorMap = (byte[])ColorMap.Clone();
DepthImagePixel[,] depthMap = (DepthImagePixel[, ])DepthMappedToColor.Clone();
int width = myKin.ColorStream.FrameWidth;
int height = myKin.ColorStream.FrameHeight;
Console.WriteLine("Measuring maximum and minimum depth for drawing...");
short maxDepth = 0;
short minDepth = 4096;
for (int i = 0; i < width; i++)
{
for (int j = 0; j < height; j++)
{
if (depthMap[i, j].Depth > maxDepth)
{
maxDepth = depthMap[i, j].Depth;
}
if (depthMap[i, j].Depth != 0 && depthMap[i, j].Depth < minDepth)
{
minDepth = depthMap[i, j].Depth;
}
}
}
Graph.cleardevice();
for (int i = 0; i < width; i++)
{
for (int j = 0; j < height; j++)
{
if (depthMap[i, j].Depth != 0)
{
byte c = (byte)(255 - (depthMap[i, j].Depth - minDepth) * 255 / (maxDepth - minDepth));
Graph.putpixel(i, j, Color.FromRGB(c, c, c));
}
}
}
Graph.delay(0);
Console.WriteLine("Please click on the most buttom of object wanted:");
int buttom, click, x, y;
do
{
Graph.getmouse(out x, out buttom, out click);
Graph.cleardevice();
for (int i = 0; i < width; i++)
{
for (int j = 0; j < buttom; j++)
{
if (depthMap[i, j].Depth != 0)
{
Graph.putpixel(i, j, GetColorPixel(i, j, colorMap));
}
}
}
Graph.delay(0);
} while (click != 1);
while (click == 1)
{
Graph.getmouse(out x, out buttom, out click);
}
for (int i = 0; i < width; i++)
{
for (int j = 0; j < buttom; j++)
{
if (depthMap[i, j].Depth != 0)
{
byte c = (byte)(255 - (depthMap[i, j].Depth - minDepth) * 255 / (maxDepth - minDepth));
Graph.putpixel(i, j, Color.FromRGB(c, c, c));
}
}
}
Graph.delay(0);
Console.WriteLine("Please select the objects and then press Enter:");
Bitmap depthPic = Graph.getimage(0, 0, width, height);
int[,] selected = new int[width, buttom];
int count = 0;
while (true)
{
do
{
Graph.putimage(depthPic, 0, 0);
Graph.getmouse(out x, out y, out click);
if (y < buttom)
{
if (depthMap[x, y].Depth != 0 && selected[x, y] == 0)
{
count++;
XYQueue queue = new XYQueue(width * buttom);
int def = 20;
queue.insert(x, y);
do
{
int x0, y0;
queue.delete(out x0, out y0);
if (selected[x0, y0] != count)
{
selected[x0, y0] = count;
if (x0 > 0 && selected[x0 - 1, y0] == 0 && depthMap[x0 - 1, y0].Depth != 0 && Math.Abs(depthMap[x0 - 1, y0].Depth - depthMap[x0, y0].Depth) <= def)
{
queue.insert(x0 - 1, y0);
}
if (x0 < width - 1 && selected[x0 + 1, y0] == 0 && depthMap[x0 + 1, y0].Depth != 0 && Math.Abs(depthMap[x0 + 1, y0].Depth - depthMap[x0, y0].Depth) <= def)
{
queue.insert(x0 + 1, y0);
}
if (y0 > 0 && selected[x0, y0 - 1] == 0 && depthMap[x0, y0 - 1].Depth != 0 && Math.Abs(depthMap[x0, y0 - 1].Depth - depthMap[x0, y0].Depth) <= def)
{
queue.insert(x0, y0 - 1);
}
if (y0 < buttom - 1 && selected[x0, y0 + 1] == 0 && depthMap[x0, y0 + 1].Depth != 0 && Math.Abs(depthMap[x0, y0 + 1].Depth - depthMap[x0, y0].Depth) <= def)
{
queue.insert(x0, y0 + 1);
}
}
} while (queue.Count != 0);
}
for (int i = 0; i < width; i++)
{
for (int j = 0; j < buttom; j++)
{
if (selected[i, j] != 0 && selected[i, j] == selected[x, y])
{
Graph.putpixel(i, j, GetColorPixel(i, j, colorMap));
}
}
}
}
for (int i = 0; i < width; i++)
{
for (int j = 0; j < buttom; j++)
{
if (selected[i, j] < 0)
{
Graph.putpixel(i, j, GetColorPixel(i, j, colorMap));
}
}
}
Graph.delay(0);
} while (click != 1 && !Graph.keydown(System.Windows.Forms.Keys.Enter));
if (Graph.keydown(System.Windows.Forms.Keys.Enter))
{
break;
}
while (click == 1)
{
Graph.getmouse(out x, out y, out click);
}
int change = selected[x, y];
if (change > 0)
{
for (int i = 0; i < width; i++)
{
for (int j = 0; j < buttom; j++)
{
if (selected[i, j] == change)
{
selected[i, j] *= -selected[i, j];
}
}
}
Console.WriteLine("Object Selected.");
}
}
Console.WriteLine("Converting to stereo image...");
int lbx = width, lby = buttom, ubx = 0, uby = 0;
for (int i = 0; i < width; i++)
{
for (int j = 0; j < buttom; j++)
{
if (selected[i, j] < 0)
{
if (lbx > i)
{
lbx = i;
}
if (lby > j)
{
lby = j;
}
if (ubx < i)
{
ubx = i;
}
if (uby < j)
{
uby = j;
}
}
}
}
int mx = (ubx + lbx) / 2;
int my = (uby + lby) / 2;
int movex = width / 2 - mx;
int movey = height / 2 - my;
Bitmap pic = new Bitmap(width * 2, height);
for (int i = 0; i < width; i++)
{
for (int j = 0; j < buttom; j++)
{
if (selected[i, j] < 0)
{
int z = (depthMap[i, j].Depth - minDepth) * scaleDepth / (4096 - minDepth);
{
double x1, x2, y0;
Graph3dDraw._to2Da(i + movex, j + movey, z, out x1, out x2, out y0);
Color c = GetColorPixel(i, j, colorMap);
pic.SetPixel((int)x1, (int)y0, c);
pic.SetPixel((int)x2 + width, (int)y0, c);
}
}
}
}
Console.WriteLine("Croping to smaller stereo image...");
int lb1 = -1, ub1 = -1, lb2 = -1, ub2 = -1;
for (int i = 0; i < width; i++)
{
int j;
for (j = 0; j < buttom; j++)
{
if (pic.GetPixel(i, j).a > 0)
{
lb1 = i;
break;
}
}
if (j < buttom)
{
break;
}
}
for (int i = width - 1; i > 0; i--)
{
int j;
for (j = 0; j < buttom; j++)
{
if (pic.GetPixel(i, j).a > 0)
{
ub1 = i;
break;
}
}
if (j < buttom)
{
break;
}
}
for (int i = 0; i < width; i++)
{
int j;
for (j = 0; j < buttom; j++)
{
if (pic.GetPixel(i + width, j).a > 0)
{
lb2 = i;
break;
}
}
if (j < buttom)
{
break;
}
}
for (int i = width - 1; i > 0; i--)
{
int j;
for (j = 0; j < buttom; j++)
{
if (pic.GetPixel(i + width, j).a > 0)
{
ub2 = i;
break;
}
}
if (j < buttom)
{
break;
}
}
for (int j = 0; j < height; j++)
{
int i;
for (i = 0; i < width * 2; i++)
{
if (pic.GetPixel(i, j).a > 0)
{
lby = j;
break;
}
}
if (i < width)
{
break;
}
}
for (int j = height - 1; j >= 0; j--)
{
int i;
for (i = 0; i < width * 2; i++)
{
if (pic.GetPixel(i, j).a > 0)
{
uby = j;
break;
}
}
if (i < width)
{
break;
}
}
int finLB = (lb1 < lb2) ? lb1 : lb2;
int finUB = (ub1 > ub2) ? ub1 : ub2;
result = new Bitmap((finUB - finLB + 1) * 2, uby - lby + 1);
int dx = finLB - lb1;
for (int i = lb1; i < ub1; i++)
{
for (int j = lby; j < uby; j++)
{
Color c = pic.GetPixel(i, j);
result.SetPixel(dx + i - lb1, j - lby, c);
}
}
dx = finUB + 1 - lb2;
for (int i = lb2; i < ub2; i++)
{
for (int j = lby; j < uby; j++)
{
Color c = pic.GetPixel(i + width, j);
result.SetPixel(dx + i - lb2, j - lby, c);
}
}
Console.WriteLine("Done");
}
return(result);
}
