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