Smoothing Kinect Depth Frames in Real-Time

using System.Threading.Tasks;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using Microsoft.Research.Kinect.Nui;

namespace KinectDepthSmoothing
{
    public partial class MainWindow : Window
    {
        private BitmapSource CreateSmoothImageFromDepthArray(ImageFrame image)
        {
            int width = image.Image.Width;
            int height = image.Image.Height;

            // We first want to create a simple array where each index represents a single pixel of depth information.
            // This will make it easier to work with the data to filter and average it for smoothing.
            short[] depthArray = CreateDepthArray(image);

            // Users can decide from the UI if this feature is applied
            if (useFiltering)
                depthArray = CreateFilteredDepthArray(depthArray, width, height);

            // Users can decide from the UI if this feature is applied
            if (useAverage)
                depthArray = CreateAverageDepthArray(depthArray);

            // After we have processed the data, we can transform it into color channels for final rendering.
            byte[] colorBytes = CreateColorBytesFromDepthArray(depthArray, width, height);


            return BitmapSource.Create(width, height, 96, 96, PixelFormats.Bgr32, null, colorBytes, width * PixelFormats.Bgr32.BitsPerPixel / 8);
        }

        private byte[] CreateColorBytesFromDepthArray(short[] depthArray, int width, int height)
        {
            // We multiply the product of width and height by 4 because each byte
            // will represent a different color channel per pixel in the final iamge.
            byte[] colorFrame = new byte[width * height * 4];

            Parallel.For(0, depthArray.Length, distanceIndex =>
            {
                // Because the colorFrame we are creating has four times as many bytes representing
                // a pixel in the final image, we set the index to be twice of the depth index.
                var index = distanceIndex * 4;

                // Map the distance to an intesity that can be represented in RGB
                var intensity = CalculateIntensityFromDistance(depthArray[distanceIndex]);

                // Apply the intensity to the color channels
                colorFrame[index + BlueIndex] = intensity;
                colorFrame[index + GreenIndex] = intensity;
                colorFrame[index + RedIndex] = intensity;
            });

            return colorFrame;
        }
    }
}