Convert Format16bppGrayScale Bitmap to BitmapImage

Question

I am working on C# application using the Intel RealSense SDK. I used this sample that is able to track people in a color frame: https://software.intel.com/en-us/articles/diy-pan-tilt-person-tracking-with-intel-realsense-camera

The sample can show the X,Y coordinates of a person's body, but not his distance unless it sees a face (Z coordinates), instead of this, I would like to also record and display the depthdata (a grayscale image) of the people on-screen. One of the steps that the sample program does before it is able to display an image on the screen is convert the ImageData from the Intel RealSense to a Format32bppArgb Bitmap, and then convert that later to a BitmapImage.

Here is the code that converts the ImageData (color) to a Format32bppArgb Bitmap, used for the ImageData from the RealSense (from the sample):

public Bitmap ToBitmap(Int32 index, Int32 width, Int32 height)
{
    Bitmap bitmap = null;
    switch (format)
    {
        case PixelFormat.PIXEL_FORMAT_RGB32:
            bitmap = new Bitmap(width, height, pitches[index], System.Drawing.Imaging.PixelFormat.Format32bppArgb, planes[0]);
            break;
        case PixelFormat.PIXEL_FORMAT_RGB24:
            bitmap = new Bitmap(width, height, pitches[index], System.Drawing.Imaging.PixelFormat.Format24bppRgb, planes[0]);
            break;
        case PixelFormat.PIXEL_FORMAT_DEPTH:
        case PixelFormat.PIXEL_FORMAT_DEPTH_RAW:
            bitmap = new Bitmap(width, height, pitches[index], System.Drawing.Imaging.PixelFormat.Format16bppGrayScale, planes[index]);
            break;
        default:
            return null;
    }
    return bitmap;
}

I created a seperate method to convert the (depth) ImageData to a Format16bppGrayScale Bitmap in which I set the format beforehand to PixelFormat.PIXEL_FORMAT_DEPTH_RAW, I use this for the DepthData.

public Bitmap ToDepthBitmap(Int32 index, Int32 width, Int32 height)
{
    Bitmap bitmap = null;
    format = PixelFormat.PIXEL_FORMAT_DEPTH;
    switch (format)
    {
        case PixelFormat.PIXEL_FORMAT_RGB32:
            bitmap = new Bitmap(width, height, pitches[index], System.Drawing.Imaging.PixelFormat.Format32bppArgb, planes[0]);
            break;
        case PixelFormat.PIXEL_FORMAT_RGB24:
            bitmap = new Bitmap(width, height, pitches[index], System.Drawing.Imaging.PixelFormat.Format24bppRgb, planes[0]);
            break;
        case PixelFormat.PIXEL_FORMAT_DEPTH:
        case PixelFormat.PIXEL_FORMAT_DEPTH_RAW:
            bitmap = new Bitmap(width, height, pitches[index], System.Drawing.Imaging.PixelFormat.Format16bppGrayScale, planes[index]);
            break;
        default:
            return null;
    }
    return bitmap;
}

Here is the code that converts the Format32bppArgb Bitmap to a BitmapImage and then shows it on the screen (from the sample):

private void Render(Bitmap bitmap, MyTrackedPerson myTrackedPerson)
{
    Dispatcher.Invoke(System.Windows.Threading.DispatcherPriority.Normal, new Action(delegate ()
    {
        BitmapImage bitmapImage = ConvertBitmap(bitmap);
        if (bitmapImage != null)
        {
            imgStream.Source = bitmapImage;
        }
    }));
}

private BitmapImage ConvertBitmap(Bitmap bitmap)
{
    BitmapImage bitmapImage = null;

    if (bitmap != null)
    {
        MemoryStream memoryStream = new MemoryStream();
        bitmap.Save(memoryStream, System.Drawing.Imaging.ImageFormat.Bmp);
        memoryStream.Position = 0;
        bitmapImage = new BitmapImage();
        bitmapImage.BeginInit();
        bitmapImage.StreamSource = memoryStream;
        bitmapImage.CacheOption = BitmapCacheOption.OnLoad;
        bitmapImage.EndInit();
        bitmapImage.Freeze();
    }

    return bitmapImage;
}

I added a seperate methods again to try to do the same steps (Format16bppGrayScale Bitmap to BitmapImage) for the DepthData stream:

private void RenderDepth(Bitmap bitmap, MyTrackedPerson myTrackedPerson)
{
    Dispatcher.Invoke(System.Windows.Threading.DispatcherPriority.Normal, new Action(delegate ()
    {
        BitmapImage bitmapImage = ConvertDepthBitmap(bitmap);
        if (bitmapImage != null)
        {
            imgStream.Source = bitmapImage;
        }
    }));
}

private BitmapImage ConvertDepthBitmap(Bitmap bitmap)
{
    BitmapImage bitmapImage = null;

    if (bitmap != null)
    {
        MemoryStream memoryStream = new MemoryStream();

        var rect = new Rectangle(0, 0, bitmap.Width, bitmap.Height);
        var bitmapCopy = (Bitmap)bitmap.Clone();
        var bitmapData = bitmap.LockBits(rect, ImageLockMode.WriteOnly, bitmap.PixelFormat);

        var numberOfBytes = bitmapData.Stride * bitmap.Height;
        var bitmapBytes = new short[bitmap.Width * bitmap.Height];

        var k = 0;

        for (int i = 0; i < bitmap.Width; i++)
        {
            for (int j = 0; j < bitmap.Height; j++)
            {
                bitmapBytes[k] = (short)bitmapCopy.GetPixel(i, j).ToArgb();
                k++;
            }
        }

        var ptr = bitmapData.Scan0;
        Marshal.Copy(bitmapBytes, 0, ptr, bitmapBytes.Length);

        bitmap.UnlockBits(bitmapData);

        bitmap.Save(memoryStream, System.Drawing.Imaging.ImageFormat.Bmp);
        memoryStream.Position = 0;
        bitmapImage = new BitmapImage();
        bitmapImage.BeginInit();
        bitmapImage.StreamSource = memoryStream;
        bitmapImage.CacheOption = BitmapCacheOption.OnLoad;
        bitmapImage.EndInit();
        bitmapImage.Freeze();
    }

    return bitmapImage;
}

This last code sample above is based on the answer of this question: Generate 16-bit grayscale BitmapData and save to file

Unfortunately, the code above gives a Argument Exception that says that a parameter is invalid on this line:

bitmapBytes[k] = (short)bitmapCopy.GetPixel(i, j).ToArgb();

Can anyone point out if I am missing something here? Am I doing it incorrectly?