Intel RealSense Camera (F200): A Bit of Depth

Posted on by

I’ve been playing around with the RealSense SDK in these previous four posts;

but I haven’t really done anything with the data as of yet other than to just ask the SDK for it as RGB data and then to hand it over to some image in WPF to get it rendered.

I thought I’d see what the depth data looks like and, as such, I reverted back to the approach that I took in the ‘Part 2’ post above in that I went back to working with the PXCMSenseManager object as it seems to bring many things together in a lot fewer lines of code than the approach that I was taking in Parts 3/4.

From the docs, the natural form of the DEPTH data is in 2-byte integer form where the values are depth values in millimetres from the camera.

I thought I’d experiment with a simple console application that could try to tell me whether something was within (e.g.) 100mm of the sensor and came up with;

namespace ConsoleApplication1
{
  using System;
  using System.Collections.Generic;
  using System.Linq;

  class PXCMStatusException : Exception
  {
    public PXCMStatusException(pxcmStatus status)
    {
      this.Status = status;
    }
    public pxcmStatus Status { get; private set; }
  }
  static class PXCMStatusExtensions
  {
    public static void ThrowOnFail(this pxcmStatus status)
    {
      if (!status.Succeeded())
      {
        throw new PXCMStatusException(status);
      }
    }
    public static bool Succeeded(this pxcmStatus status)
    {
      return (status == pxcmStatus.PXCM_STATUS_NO_ERROR);
    }
  }
  class Program
  {
    static bool UnsafeScanForMinimumDistanceMillimetres(
      PXCMImage.ImageData imageData,
      UInt16 minimumDistanceMm,
      ulong length)
    {
      bool found = false;

      unsafe
      {
        UInt16* ptr = (UInt16*)imageData.planes[0].ToPointer();

        for (ulong i = 0; ((i < length) && !found); i++, ptr++)
        {
          found = (*ptr > 0) && (*ptr < minimumDistanceMm);
        }
      }
      return (found);
    }
    static void Main(string[] args)
    {
      const int minimumDistance = 300; // mm

      Console.WriteLine("Hit a key to end...");

      using (PXCMSenseManager senseManager = PXCMSenseManager.CreateInstance())
      {
        // I don't mind dropping frames that arrive while I'm processing the current frame -
        // that is, the system can drop frames that arrive in between my AquireFrame() ->
        // ReleaseFrame() calls, I'll live with it. Only learnt about this 'realtime'
        // mode which I should have perhaps known about previously! 🙂
        senseManager.captureManager.SetRealtime(false);

        senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, 0, 0).ThrowOnFail();

        senseManager.Init().ThrowOnFail();

        while (!Console.KeyAvailable)
        {
          if (senseManager.AcquireFrame().Succeeded())
          {
            PXCMCapture.Sample sample = senseManager.QuerySample();
            PXCMImage.ImageData imageData;

            sample.depth.AcquireAccess(
              PXCMImage.Access.ACCESS_READ, 
              PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, 
              out imageData).ThrowOnFail();

            if (UnsafeScanForMinimumDistanceMillimetres(
              imageData, 
              minimumDistance,
              (ulong)(sample.depth.info.width * sample.depth.info.height)))
            {
              Console.WriteLine("{0:HH:mm:ss:ff}, Saw something within 100mm of the camera, PANIC!",
                DateTime.Now);
            }

            sample.depth.ReleaseAccess(imageData);

            senseManager.ReleaseFrame();
          }
        }
      }
    }
  }
}

and this is going through the steps of;

  1. Make a PXCMSenseManager instance
  2. Ask it to enable the depth stream
  3. Initialise it
  4. Wait for a frame of data via AcquireFrame (not 100% sure on this step)
  5. Access the depth image data
  6. Attempt to run a quick filter over it looking for anything that claims to be within 100mm of the camera.

and I can wave my hand around in front of the camera to see this in action;

consoleshot1

and it all seems to work quite nicely. I wanted to then make this graphical again and so I conjured up another WPF application with a simple UI of an image and a slider;

<Window x:Class="WpfApplication2.MainWindow"
        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
        Title="MainWindow"
        Height="350"
        Width="525">
  <Grid>
    <Grid.RowDefinitions>
      <RowDefinition />
      <RowDefinition Height="Auto" />
    </Grid.RowDefinitions>
    <Image x:Name="displayImage" />
    <StackPanel Grid.Row="1"
                Margin="10">
      <Slider x:Name="slider"
              Minimum="100"
              Maximum="1000"
              Margin="10" 
              SmallChange="10"
              LargeChange="50"
              ValueChanged="OnSliderValueChanged"/>
      <TextBlock HorizontalAlignment="Center"
                 TextAlignment="Center">
        <Run>Maximum Distance (mm) - currently </Run>
        <Run Text="{Binding ElementName=slider,Path=Value}" />
        <Run>mm</Run>
      </TextBlock>
    </StackPanel>
  </Grid>
</Window>

and then added a couple of little helper classes again so that I could deal with the pxcmStatus values that the SDK loves so much Smile

namespace WpfApplication2
{
  using System;

  class PXCMStatusException : Exception
  {
    public PXCMStatusException(pxcmStatus status)
    {
      this.Status = status;
    }
    public pxcmStatus Status { get; private set; }
  }
  static class PXCMStatusExtensions
  {
    public static void ThrowOnFail(this pxcmStatus status)
    {
      if (!status.Succeeded())
      {
        throw new PXCMStatusException(status);
      }
    }
    public static bool Succeeded(this pxcmStatus status)
    {
      return (status == pxcmStatus.PXCM_STATUS_NO_ERROR);
    }
  }
}

and then put some code behind my UI such that it displays the data coming from the depth camera but it filters it out to only include the data that is within a certain range (defined in mm) of the camera. I ended up with;

namespace WpfApplication2
{
  using System;
  using System.Windows;
  using System.Windows.Media;
  using System.Windows.Media.Imaging;

  public partial class MainWindow : Window
  {
    public MainWindow()
    {
      InitializeComponent();
      this.writeableBitmap = new Lazy<WriteableBitmap>(OnCreateWriteableBitmap);

      this.Loaded += OnLoaded;
    }
    void OnLoaded(object sender, RoutedEventArgs e)
    {
      this.senseManager = PXCMSenseManager.CreateInstance();

      senseManager.captureManager.SetRealtime(false);

      senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, 0, 0).ThrowOnFail();

      senseManager.Init(
        new PXCMSenseManager.Handler()
        {
          onNewSample = this.OnNewSample
        }).ThrowOnFail();

      senseManager.StreamFrames(false);
    }
    pxcmStatus OnNewSample(int mid, PXCMCapture.Sample sample)
    {
      // this is not the UI thread.
      PXCMImage.ImageData imageData;

      if (sample.depth.AcquireAccess(
        PXCMImage.Access.ACCESS_READ_WRITE, 
        PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, 
        out imageData)
        .Succeeded())
      {
        if (!this.imageDimensions.HasArea)
        {
          this.imageDimensions.Width = sample.depth.info.width;
          this.imageDimensions.Height = sample.depth.info.height;
        }

        this.FilterAndScale(
          imageData, 
          this.minimumDistanceMm,
          (ulong)(this.imageDimensions.Width * this.imageDimensions.Height));

        Dispatcher.InvokeAsync(() =>
          {
            this.writeableBitmap.Value.WritePixels(
              this.imageDimensions,
              imageData.planes[0],
              this.imageDimensions.Width * this.imageDimensions.Height * 2,
              this.imageDimensions.Width * 2);   

            // tbh - ok to release this from dispatcher thread when I acquired it
            // on a different thread?
            sample.depth.ReleaseAccess(imageData);
          }
        );
      }
      return (pxcmStatus.PXCM_STATUS_NO_ERROR);
    }
    void FilterAndScale(PXCMImage.ImageData imageData, 
      UInt16 filterMinimumValueMm,
      ulong length)
    {
      unsafe
      {
        UInt16* ptr = (UInt16*)imageData.planes[0].ToPointer();

        for (ulong i = 0; (i < length); i++, ptr++)
        {
          if (*ptr >= filterMinimumValueMm)
          {
            *ptr = 0;
          }
          else if (*ptr != 0)
          {
            *ptr = Math.Min(MAX_DISTANCE_CLAMP_MM, *ptr);
            *ptr = (UInt16)((double)*ptr / (double)MAX_DISTANCE_CLAMP_MM * UInt16.MaxValue);
          }
        }
      }
    }
    void OnSliderValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
    {
      this.minimumDistanceMm = (UInt16)e.NewValue;
    }
    WriteableBitmap OnCreateWriteableBitmap()
    {
      var bitmap = new WriteableBitmap(
          this.imageDimensions.Width,
          this.imageDimensions.Height,
          96,
          96,
          PixelFormats.Gray16,
          null);

      this.displayImage.Source = bitmap;

      return (bitmap);
    }
    const UInt16 MAX_DISTANCE_CLAMP_MM = 1000;
    UInt16 minimumDistanceMm;
    Int32Rect imageDimensions;
    Lazy<WriteableBitmap> writeableBitmap;
    PXCMSenseManager senseManager;
  }
}

and this is (attempting) to do more or less the same thing as the console application except that;

  1. it receives data in an event-driven manner via the PXCMSenseManager.Init call rather than trying to call AcquireFrame.
  2. in order to line up with WPF’s image drawing capabilities (and to avoid asking for the depth data in two formats), the code attempts to scale the depth values (which it assumes range from 0 to 1000mm) to fit into a grey-scale spectrum of 0…UInt16.MaxValue while also filtering out any values that exceed the maximum distance threshold as defined by the slider in the UI.
  3. the code has to deal with the hassle of handling UI threads and dispatchers and I made the cheap decision here to transition to the UI thread for every new image frame that I process which might not be the best plan.

I can then run this code and slide the slider from a maximum distance of 100mm to 500mm+ and gradually ‘reveal’ myself as being in front of the camera (presumably, nose first!);

ui2

 

ui3

 

ui4

and that seems to work quite nicely. I’ve dropped the code for the WPF application here in case anyone wants it