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 CreateImageFromDepthImage(ImageFrame image)
        {
            int width = image.Image.Width;
            int height = image.Image.Height;

            var depthFrame = image.Image.Bits;
            // 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.
            var colorFrame = new byte[height * width * 4];

            // We use the previously built depthIndexes IEnumerable collection for this purpose
            // For each pixel in the final image, there are two bytes that represent the depth in
            // the Kinect depth image.  To utilize the Parallel ForEach loop efficiently, we use a
            // prebuilt collection of primary indexes for these positions.
            Parallel.ForEach(depthIndexes, depthIndex =>
            {
                // Because the colorFrame we are creating has twice as many bytes representing
                // a pixel in the final image, we set the index to be twice of the depth index.
                var index = depthIndex * 2;

                // Calculate the distance represented by the two depth bytes
                var distance = CalculateDistanceFromDepth(depthFrame[depthIndex], depthFrame[depthIndex + 1]);

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

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

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