private DepthFrame cropImage(FloodFillImageData floodFillData, int width, short[] rawDepth)
{
int startX = floodFillData.rect[0];
int startY = floodFillData.rect[1];
int endX = floodFillData.rect[2];
int endY = floodFillData.rect[3];
int croppedWidth = (endX - startX + 1);
int croppedHeight = (endY - startY + 1);
short[] filledCrop = new short[croppedHeight * croppedWidth];
foreach (int i in floodFillData.l)
{
int x = i % width;
int y = i / width;
int offset = startY * width + startX;
int transposedX = x - startX;
int transposedY = y - startY;
int transposedIndex = transposedX + transposedY * croppedWidth;
filledCrop[transposedIndex] = rawDepth[i];
}
return(new DepthFrame()
{
Pixels = filledCrop, Width = croppedWidth, Height = croppedHeight
});
}
public DepthFrame Process(short[] depthPixels, int height, int width)
{
short[] secondDepthPixels = null;
if (activateSecondHand)
{
secondDepthPixels = (short[])depthPixels.Clone();
}
// initialize flood fill data
FloodFillImageData floodFillData = new FloodFillImageData();
int closestIndex = this.FindClosestPixel(depthPixels, new HashSet <int>(floodFillData.l), out this.closestDistance);
int[] rect = new int[4];
rect[0] = rect[2] = closestIndex % width;
rect[1] = rect[3] = closestIndex / width;
floodFillData.rect = rect;
//Perform flood fill on the current frame and find the cropping area.
DoFill(depthPixels, width, height, closestIndex, floodFillData);
if (activateSecondHand)
{
int secondClosestIndex = this.FindClosestPixel(secondDepthPixels, new HashSet <int>(floodFillData.l), out this.secondClosestDistance);
if (Math.Abs(this.secondClosestDistance - this.closestDistance) < CLOSEST_DEPTH_DISTANCE)
{
DoFill(secondDepthPixels, width, height, secondClosestIndex, floodFillData);
}
}
this.cropStartX = floodFillData.rect[0];
this.cropStartY = floodFillData.rect[1];
return(cropImage(floodFillData, width, depthPixels));
}
public DepthFrame Process(short[] depthPixels, int height, int width)
{
short[] secondDepthPixels = null;
if (activateSecondHand) {
secondDepthPixels = (short[])depthPixels.Clone();
}
// initialize flood fill data
FloodFillImageData floodFillData = new FloodFillImageData();
int closestIndex = this.FindClosestPixel(depthPixels, new HashSet<int>(floodFillData.l), out this.closestDistance);
int[] rect = new int[4];
rect[0] = rect[2] = closestIndex % width;
rect[1] = rect[3] = closestIndex / width;
floodFillData.rect = rect;
//Perform flood fill on the current frame and find the cropping area.
DoFill(depthPixels, width, height, closestIndex, floodFillData);
if (activateSecondHand)
{
int secondClosestIndex = this.FindClosestPixel(secondDepthPixels, new HashSet<int>(floodFillData.l), out this.secondClosestDistance);
if (Math.Abs(this.secondClosestDistance - this.closestDistance) < CLOSEST_DEPTH_DISTANCE)
DoFill(secondDepthPixels, width, height, secondClosestIndex, floodFillData);
}
this.cropStartX = floodFillData.rect[0];
this.cropStartY = floodFillData.rect[1];
return cropImage(floodFillData, width, depthPixels);
}
// Perform flood fill algorithm on the current frame
// width and height correspond to those of the current frame.
private void DoFill(short[] rawDepth, int width, int height, int closestIndex, FloodFillImageData floodFillData)
{
Queue <int> q = new Queue <int>();
List <int> l = floodFillData.l;
HashSet <int> discovered = new HashSet <int>(floodFillData.l);
int[] rect = floodFillData.rect;
int count = 0;
q.Enqueue(closestIndex);
while (q.Count != 0 && count < MAX_SIZE)
{
int n = q.Dequeue();
int x = n % width;
int y = n / width;
// check if the index is already discovered
if (!discovered.Contains(n))
{
// check if the index is within the range
if (0 <= x && x < width && 0 <= y && y < height)
{
// check if the index is within the distance
if ((ushort)rawDepth[n] - (ushort)rawDepth[closestIndex] <= HAND_DEPTH)
{
rect[0] = Math.Min(x, rect[0]);
rect[1] = Math.Min(y, rect[1]);
rect[2] = Math.Max(x, rect[2]);
rect[3] = Math.Max(y, rect[3]);
l.Add(n);
count++;
if (SPREAD_METHOD == 0)
{
// add node in each direction
if (!discovered.Contains(y * width + x + 1))
{
q.Enqueue(y * width + x + 1); // east
}
if (!discovered.Contains(y * width + x - 1))
{
q.Enqueue(y * width + x - 1); // west
}
if (!discovered.Contains((y + 1) * width + x))
{
q.Enqueue((y + 1) * width + x); // south
}
if (!discovered.Contains((y - 1) * width + x))
{
q.Enqueue((y - 1) * width + x); // north
}
}
else if (SPREAD_METHOD == 1)
{
LinkedList <int> linked = new LinkedList <int>();
Spread(y * width + x + 1, n, linked, discovered, rawDepth);
Spread(y * width + x - 1, n, linked, discovered, rawDepth);
Spread((y + 1) * width + x, n, linked, discovered, rawDepth);
Spread((y - 1) * width + x, n, linked, discovered, rawDepth);
for (LinkedListNode <int> it = linked.First; it != null; it = it.Next)
{
q.Enqueue(it.Value);
}
}
}
}
}
discovered.Add(n);
}
floodFillData.rect = rect;
}
private DepthFrame cropImage(FloodFillImageData floodFillData, int width, short[] rawDepth)
{
int startX = floodFillData.rect[0];
int startY = floodFillData.rect[1];
int endX = floodFillData.rect[2];
int endY = floodFillData.rect[3];
int croppedWidth = (endX - startX + 1);
int croppedHeight = (endY - startY + 1);
short[] filledCrop = new short[croppedHeight * croppedWidth];
foreach (int i in floodFillData.l)
{
int x = i % width;
int y = i / width;
int offset = startY * width + startX;
int transposedX = x - startX;
int transposedY = y - startY;
int transposedIndex = transposedX + transposedY * croppedWidth;
filledCrop[transposedIndex] = rawDepth[i];
}
return new DepthFrame() { Pixels = filledCrop, Width = croppedWidth, Height = croppedHeight };
}
// Perform flood fill algorithm on the current frame
// width and height correspond to those of the current frame.
private void DoFill(short[] rawDepth, int width, int height, int closestIndex, FloodFillImageData floodFillData)
{
Queue<int> q = new Queue<int>();
List<int> l = floodFillData.l;
HashSet<int> discovered = new HashSet<int>(floodFillData.l);
int[] rect = floodFillData.rect;
int count = 0;
q.Enqueue(closestIndex);
while (q.Count != 0 && count < MAX_SIZE)
{
int n = q.Dequeue();
int x = n % width;
int y = n / width;
// check if the index is already discovered
if (!discovered.Contains(n))
{
// check if the index is within the range
if (0 <= x && x < width && 0 <= y && y < height)
{
// check if the index is within the distance
if ((ushort)rawDepth[n] - (ushort)rawDepth[closestIndex] <= HAND_DEPTH)
{
rect[0] = Math.Min(x, rect[0]);
rect[1] = Math.Min(y, rect[1]);
rect[2] = Math.Max(x, rect[2]);
rect[3] = Math.Max(y, rect[3]);
l.Add(n);
count++;
if (SPREAD_METHOD == 0)
{
// add node in each direction
if (!discovered.Contains(y * width + x + 1))
q.Enqueue(y * width + x + 1); // east
if (!discovered.Contains(y * width + x - 1))
q.Enqueue(y * width + x - 1); // west
if (!discovered.Contains((y + 1) * width + x))
q.Enqueue((y + 1) * width + x); // south
if (!discovered.Contains((y - 1) * width + x))
q.Enqueue((y - 1) * width + x); // north
}
else if (SPREAD_METHOD == 1)
{
LinkedList<int> linked = new LinkedList<int>();
Spread(y * width + x + 1, n, linked, discovered, rawDepth);
Spread(y * width + x - 1, n, linked, discovered, rawDepth);
Spread((y + 1) * width + x, n, linked, discovered, rawDepth);
Spread((y - 1) * width + x, n, linked, discovered, rawDepth);
for (LinkedListNode<int> it = linked.First; it != null; it = it.Next)
{
q.Enqueue(it.Value);
}
}
}
}
}
discovered.Add(n);
}
floodFillData.rect = rect;
}