Smoothing Kinect Depth Frames in Real-Time

using System.Threading.Tasks;
using System.Windows;

namespace KinectDepthSmoothing
{
    public partial class MainWindow : Window
    {
        private short[] CreateFilteredDepthArray(short[] depthArray, int width, int height)
        {
            /////////////////////////////////////////////////////////////////////////////////////
            // I will try to comment this as well as I can in here, but you should probably refer
            // to my Code Project article for a more in depth description of the method.
            /////////////////////////////////////////////////////////////////////////////////////

            short[] smoothDepthArray = new short[depthArray.Length];

            // We will be using these numbers for constraints on indexes
            int widthBound = width - 1;
            int heightBound = height - 1;

            // We process each row in parallel
            Parallel.For(0,240, depthArrayRowIndex =>
            {
                // Process each pixel in the row
                for (int depthArrayColumnIndex = 0; depthArrayColumnIndex < 320; depthArrayColumnIndex++)
                {
                    var depthIndex = depthArrayColumnIndex + (depthArrayRowIndex * 320);

                    // We are only concerned with eliminating 'white' noise from the data.
                    // We consider any pixel with a depth of 0 as a possible candidate for filtering.
                    if (depthArray[depthIndex] == 0)
                    {
                        // From the depth index, we can determine the X and Y coordinates that the index
                        // will appear in the image.  We use this to help us define our filter matrix.
                        int x = depthIndex % 320;
                        int y = (depthIndex - x) / 320;

                        // The Sum variable is used to accumulate values per pixel for possible averaging.
                        int Sum = 0;

                        // The inner and outer band counts are used later to compare against the threshold 
                        // values set in the UI to identify a positive filter result.
                        int innerBandCount = 0;
                        int outerBandCount = 0;

                        // The following loops will loop through a 5 X 5 matrix of pixels surrounding the 
                        // candidate pixel.  This defines 2 distinct 'bands' around the candidate pixel.
                        // If any of the pixels in this matrix are non-0, we will accumulate them and count
                        // how many non-0 pixels are in each band.  If the number of non-0 pixels breaks the
                        // threshold in either band, then the average of all non-0 pixels in the matrix is applied
                        // to the candidate pixel.
                        for (int yi = 0; yi < 3; yi++)
                        {
                            for (int xi = 0; xi < 3; xi++)
                            {
                                // yi and xi are modifiers that will be subtracted from and added to the
                                // candidate pixel's x and y coordinates that we calculated earlier.  From the
                                // resulting coordinates, we can calculate the index to be addressed for processing.

                                // We do not want to consider the candidate pixel (xi = 0, yi = 0) in our process at this point.
                                // We already know that it's 0
                                if (xi != 0 || yi != 0)
                                {
                                    // We then create our modified coordinates for each pass
                                    var xAdd = x + xi;
                                    var xSub = x - xi;
                                    var yAdd = y + yi;
                                    var ySub = y - yi;

                                    // While the modified coordinates may in fact calculate out to an actual index, it 
                                    // might not be the one we want.  Be sure to check to make sure that the modified coordinates
                                    // match up with our image bounds.
                                    if (xAdd >= 0 && xAdd <= widthBound && yAdd >= 0 && yAdd <= heightBound)
                                    {
                                        var index = xAdd + (yAdd * width);
                                        if (depthArray[index] != 0)
                                        {
                                            Sum += depthArray[index];
                                            if (yi != 2 && xi != 2)
                                                innerBandCount++;
                                            else
                                                outerBandCount++;
                                        }
                                    }

                                    // We are already able to calculate xi = 0 in the previous lines.  We don't want to count
                                    // the same thing twice.
                                    if (xi != 0)
                                    {
                                        if (xSub >= 0 && xSub <= widthBound && yAdd >= 0 && yAdd <= heightBound)
                                        {
                                            var index = xSub + (yAdd * width);
                                            if (depthArray[index] != 0)
                                            {
                                                Sum += depthArray[index];
                                                if (yi != 2 && xi != 2)
                                                    innerBandCount++;
                                                else
                                                    outerBandCount++;
                                            }
                                        }
                                    }

                                    // We are already able to calculate yi = 0 in the previous lines.  We don't want to count
                                    // the same thing twice.
                                    if (yi != 0)
                                    {
                                        if (xAdd >= 0 && xAdd <= widthBound && ySub >= 0 && ySub <= heightBound)
                                        {
                                            var index = xAdd + (ySub * width);
                                            if (depthArray[index] != 0)
                                            {
                                                Sum += depthArray[index];
                                                if (yi != 2 && xi != 2)
                                                    innerBandCount++;
                                                else
                                                    outerBandCount++;
                                            }
                                        }

                                        // We are already able to calculate xi = 0 in the previous lines.  We don't want to count
                                        // the same thing twice.
                                        if (xi != 0)
                                        {
                                            if (xSub >= 0 && xSub <= widthBound && ySub >= 0 && ySub <= heightBound)
                                            {
                                                var index = xSub + (ySub * width);
                                                if (depthArray[index] != 0)
                                                {
                                                    Sum += depthArray[index];
                                                    if (yi != 2 && xi != 2)
                                                        innerBandCount++;
                                                    else
                                                        outerBandCount++;
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }

                        // Once we have determined our inner and outer band non-zero counts, and accumulated all of those values,
                        // we can compare it against the threshold to determine if our candidate pixel will be changed to the
                        // average of the non-zero surrounding pixels.
                        if (innerBandCount >= innerBandThreshold || outerBandCount >= outerBandThreshold)
                            smoothDepthArray[depthIndex] = (short)(Sum / (innerBandCount + outerBandCount));

                    }
                    else
                    {
                        // If the pixel is not zero, we will keep the original depth.
                        smoothDepthArray[depthIndex] = depthArray[depthIndex];
                    }
                }
            });

            return smoothDepthArray;
        }
    }
}