Intel RealSense Camera (F200): ‘Hello World’ Part 4

Following up on my previous post, I wanted to put some of what I’d figured out back behind something that I could at least point and click my way through and so I made a ‘UI’ in WPF for selecting a stream and viewing it;

Capture

shot2

In doing so, I stuck to the approach that I’d taken in the previous post of enumerating modules, devices, streaming profiles but I wanted to tidy things up at least a little by having a UI which is data-bound and so this is just 3 ListBoxes and an Image with a viewmodel called, imaginatively ViewModel;

<Window x:Class="HelloRealSense.MainWindow"
        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
        xmlns:local="clr-namespace:HelloRealSense"
        Title="MainWindow"
        Height="768"
        Width="1024">
  <Window.DataContext>
    <local:ViewModel />
  </Window.DataContext>
  <Window.Resources>
    <DataTemplate x:Key="nameTemplate">
      <TextBlock Text="{Binding Name}" />
    </DataTemplate>
  </Window.Resources>
  <Grid>
    <Grid.RowDefinitions>
      <RowDefinition Height="2*" />
      <RowDefinition Height="*" />
    </Grid.RowDefinitions>
    <Grid>
      <Image Source="{Binding ImageSource}" VerticalAlignment="Center" HorizontalAlignment="Center"
             Stretch="None"/>
    </Grid>
    <Grid Grid.Row="1">
      <Grid.ColumnDefinitions>
        <ColumnDefinition Width="Auto" />
        <ColumnDefinition Width="Auto" />
        <ColumnDefinition />
      </Grid.ColumnDefinitions>
      <ListBox ItemsSource="{Binding CaptureModuleDescriptions}"
               SelectedItem="{Binding SelectedCaptureModuleDescription,Mode=TwoWay}"
               SelectionMode="Single"
               ItemTemplate="{StaticResource nameTemplate}">
      </ListBox>
      <ListBox ItemsSource="{Binding DeviceDescriptions}"
               SelectedItem="{Binding SelectedDeviceDescription,Mode=TwoWay}"
               SelectionMode="Single"
               Grid.Column="1"
               ItemTemplate="{StaticResource nameTemplate}">
      </ListBox>
      <ListBox ItemsSource="{Binding DeviceStreamingProfiles}"
               SelectedItem="{Binding SelectedStreamProfile,Mode=TwoWay}"
               SelectionMode="Single"
               Grid.Column="2"
               ItemTemplate="{StaticResource nameTemplate}">
      </ListBox>
    </Grid>

  </Grid>
</Window>

I wrote a few more ‘helpers’ in putting this together. I wanted something that dealt a little with status codes for me although I stayed away from turning them into exceptions but I may end up there in the future;

namespace HelloRealSense
{
  static class StatusHelper
  {
    public static bool Succeeded(this pxcmStatus status)
    {
      return (status == pxcmStatus.PXCM_STATUS_NO_ERROR);
    }
    public static bool Failed(this pxcmStatus status)
    {
      return (!status.Succeeded());
    }
  }
}

and I wanted a class that abstracted out some of the SDK-mechanism for querying/enumerating although I’m still not sure I’m 100% happy with this, it’s still too ‘wordy’ but I find it better than attempting to use the SDK in the raw;

namespace HelloRealSense
{
  using System.Collections.Generic;

  delegate pxcmStatus RSQueryWithDescriptionAndReturnTypeIteratorFunction<D,T>(
    D descriptionType, int index, out T returnType);

  delegate pxcmStatus RSQueryWithDescriptionIteratorFunction<T>(
    T descriptionType, int index, out T returnType);

  delegate pxcmStatus RSQueryIteratorFunction<T>(
    int index, out T returnType);

  static class RSEnumerationHelper
  {
    public static IEnumerable<T> QueryValuesWithDescription<D,T>(D description,
      RSQueryWithDescriptionAndReturnTypeIteratorFunction<D,T> queryIterator)
    {
      int i = 0;
      T current;

      while (queryIterator(description, i++, out current) == pxcmStatus.PXCM_STATUS_NO_ERROR)
      {
        yield return current;
      }
    }
    public static IEnumerable<T> QueryValuesWithDescription<T>(T description,
      RSQueryWithDescriptionIteratorFunction<T> queryIterator)
    {
      int i = 0;
      T current;

      while (queryIterator(description, i++, out current) == pxcmStatus.PXCM_STATUS_NO_ERROR)
      {
        yield return current;
      }
    }
    public static IEnumerable<T> QueryValues<T>(RSQueryIteratorFunction<T> queryIterator)
    {
      int i = 0;
      T current;

      while (queryIterator(i++, out current) == pxcmStatus.PXCM_STATUS_NO_ERROR)
      {
        yield return current;
      }
    }
  }
}

and I wrote an extension to the PXCMSample class so as to extract from it the image that belongs to a particular type of stream (PXCMCapture.StreamType);

namespace HelloRealSense
{
  static class PXCMSampleExtensions
  {
    public static PXCMImage GetImageForStreamType(this PXCMCapture.Sample sample, PXCMCapture.StreamType streamType)
    {
      PXCMImage image = null;

      switch (streamType)
      {
        case PXCMCapture.StreamType.STREAM_TYPE_ANY:
          break;
        case PXCMCapture.StreamType.STREAM_TYPE_COLOR:
          image = sample.color;
          break;
        case PXCMCapture.StreamType.STREAM_TYPE_DEPTH:
          image = sample.depth;
          break;
        case PXCMCapture.StreamType.STREAM_TYPE_IR:
          image = sample.ir;
          break;
        case PXCMCapture.StreamType.STREAM_TYPE_LEFT:
          image = sample.left;
          break;
        case PXCMCapture.StreamType.STREAM_TYPE_RIGHT:
          image = sample.right;
          break;
        default:
          break;
      }
      return (image);
    }
  }
}

and I ended up writing some very basic ‘view model’ classes for three of the types in the SDK (PXCMSession.ImplDesc, PXCMCapture.DeviceInfo, PXCMDevice.StreamProfileSet). Wanting to be cheap, I’d hoped that I could avoid ‘view models’ for these types and just bind directly to them from my XAML but the SDK tends to have types that offer public fields which don’t work for binding and so I wrote these tiny wrappers around these types just to make them useful to binding;

namespace HelloRealSense
{

  class CaptureModuleViewModel 
  {
    public CaptureModuleViewModel(PXCMSession.ImplDesc implDesc)
    {
      this.implDesc = implDesc;
    }
    public string Name
    {
      get
      {
        return (this.implDesc.friendlyName);
      }
    }
    public PXCMSession.ImplDesc ImplDesc
    {
      get
      {
        return (this.implDesc);
      }
    }
    PXCMSession.ImplDesc implDesc;
  }
}

and

namespace HelloRealSense
{

  class DeviceDescriptionViewModel
  {
    public DeviceDescriptionViewModel(PXCMCapture.DeviceInfo deviceInfo)
    {
      this.deviceInfo = deviceInfo;
    }
    public string Name
    {
      get
      {
        return (this.deviceInfo.name);
      }
    }
    public PXCMCapture.DeviceInfo DeviceInfo
    {
      get
      {
        return (this.deviceInfo);
      }
    }
    PXCMCapture.DeviceInfo deviceInfo;
  }
}

and

namespace HelloRealSense
{
  class StreamProfileViewModel
  {
    public StreamProfileViewModel(PXCMCapture.Device.StreamProfileSet streamProfileSet,
      PXCMCapture.StreamType streamType)
    {
      this.streamProfileSet = streamProfileSet;
      this.streamType = streamType;
    }
    public string Name
    {
      get
      {
        return (string.Format(
          "{0} ({1} at {2} Hz [{3}x{4}])",
          this.streamType,
          this.streamProfileSet[this.streamType].imageInfo.format,
          this.streamProfileSet[this.streamType].frameRate.max,
          this.streamProfileSet[this.streamType].imageInfo.width,
          this.streamProfileSet[this.streamType].imageInfo.height));
      }
    }
    public PXCMCapture.StreamType StreamType
    {
      get
      {
        return (this.streamType);
      }
    }
    public PXCMCapture.Device.StreamProfileSet StreamProfileSet
    {
      get
      {
        return (this.streamProfileSet);
      }
    }
    PXCMCapture.StreamType streamType;
    PXCMCapture.Device.StreamProfileSet streamProfileSet;
  }
}

and, finally, I wanted a little helper class that took on the job of calling the ReadStreams method of the PXCMCapture.Device class to read the frames of streaming data in a way that was at least ‘compatible’ with using the async/await approach of modern .NET.

Interestingly, that PXCMCapture.Device class has a ReadStreamsAsync method on it but I found that calling that seemed to result in an ‘unsupported’ error and so I wrote this little class which offers a GetNextSampleAsync() method that’s intended to be called in a loop from the UI thread using the await pattern to restore the syncrhonisation context in order to then take action and display the data in an image on-screen;

namespace HelloRealSense
{
  using System.Threading.Tasks;

  class ImageReader
  {
    public ImageReader(
      PXCMCapture.Device device,
      PXCMCapture.StreamType streamType)
    {
      this.device = device;
      this.streamType = streamType;
    }
    public Task<PXCMCapture.Sample> GetNextSampleAsync()
    {
      // NB: taking the simple but possibly wasteful approach of acquiring each
      // frame on a separate task. The SDK offers a ReadStreamsAsync method but
      // all I get back from that is an 'unsupported' error code. I could come
      // up with a better 'architecture' here but, so far, this seems to work
      // ok.
      Task<PXCMCapture.Sample> task = Task.Factory.StartNew<PXCMCapture.Sample>(
        () =>
        {
          PXCMCapture.Sample sample = new PXCMCapture.Sample();

          if (!this.device.ReadStreams(this.streamType, sample).Succeeded())
          {
            sample = null;
          }
          return (sample);
        });

      return (task);
    }
    PXCMCapture.StreamType streamType;
    PXCMCapture.Device device;
  }
}

With that in play, I could write the main ViewModel class behind the UI which is fairly simple other than in a couple of places that I had to spend a bit of time thinking about. The class possibly got a bit ‘large’ and needs re-factoring but it’s as below (it derives from a ViewModelBase class which implements INotifyPropertyChanged for it);

namespace HelloRealSense
{
  using System;
  using System.Collections.Generic;
  using System.Linq;
  using System.Threading;
  using System.Threading.Tasks;
  using System.Windows;
  using System.Windows.Media;
  using System.Windows.Media.Imaging;

  class ViewModel : ViewModelBase
  {
    public ViewModel()
    {
      this.session = new Lazy<PXCMSession>(() => PXCMSession.CreateInstance());
    }
    public WriteableBitmap ImageSource
    {
      get
      {
        return (this.imageSource);
      }
      private set
      {
        base.SetProperty(ref this.imageSource, value);
      }
    }
    public IEnumerable<CaptureModuleViewModel> CaptureModuleDescriptions
    {
      get
      {
        var implDescs = RSEnumerationHelper.QueryValuesWithDescription(
          new PXCMSession.ImplDesc()
          {
            group = PXCMSession.ImplGroup.IMPL_GROUP_SENSOR,
            subgroup = PXCMSession.ImplSubgroup.IMPL_SUBGROUP_VIDEO_CAPTURE
          },
          this.session.Value.QueryImpl
        );
        var viewModels = implDescs.Select(i => new CaptureModuleViewModel(i));

        return (viewModels);
      }
    }
    public IEnumerable<DeviceDescriptionViewModel> DeviceDescriptions
    {
      get
      {
        IEnumerable<DeviceDescriptionViewModel> viewModels = null;

        if (this.selectedCaptureModule != null)
        {
          var descriptions = RSEnumerationHelper.QueryValues<PXCMCapture.DeviceInfo>(
            this.selectedCaptureModule.QueryDeviceInfo);

          viewModels = descriptions.Select(d => new DeviceDescriptionViewModel(d));
        }
        return (viewModels);
      }
    }
    public IEnumerable<StreamProfileViewModel> DeviceStreamingProfiles
    {
      get
      {
        IEnumerable<StreamProfileViewModel> viewModels = new List<StreamProfileViewModel>();

        if (this.selectedCaptureDevice != null)
        {
          for (int i = 0; i < PXCMCapture.STREAM_LIMIT; i++)
          {
            var streamType = PXCMCapture.StreamTypeFromIndex(i);

            var profilesForStreamType =
              RSEnumerationHelper.QueryValuesWithDescription<PXCMCapture.StreamType, PXCMCapture.Device.StreamProfileSet>(
                streamType, this.selectedCaptureDevice.QueryStreamProfileSet);

            viewModels = viewModels.Concat(
              profilesForStreamType.Select(p => new StreamProfileViewModel(p, streamType)));
          }
        }
        return (viewModels);
      }
    }
    public DeviceDescriptionViewModel SelectedDeviceDescription
    {
      get
      {
        return (this.selectedDeviceDescription);
      }
      set
      {
        base.SetProperty(ref this.selectedDeviceDescription, value);
        this.SelectedDeviceDescriptionChanged();
      }
    }
    void SelectedDeviceDescriptionChanged()
    {
      if (this.selectedCaptureDevice != null)
      {
        this.selectedCaptureDevice.Dispose();
        this.selectedCaptureDevice = null;
      }
      if (this.selectedDeviceDescription != null)
      {
        this.RecreateDevice();
      }
      base.OnPropertyChanged("DeviceStreamingProfiles");
    }
    public CaptureModuleViewModel SelectedCaptureModuleDescription
    {
      get
      {
        return (this.selectedCaptureModuleDescription);
      }
      set
      {
        base.SetProperty(ref this.selectedCaptureModuleDescription, value);
        this.SelectedModuleDescriptionChanged();
      }
    }
    void SelectedModuleDescriptionChanged()
    {
      if (this.selectedCaptureModule != null)
      {
        this.selectedCaptureModule.Dispose();
        this.selectedCaptureModule = null;
      }
      if (this.selectedCaptureModuleDescription != null)
      {
        PXCMCapture localCapture;

        if (this.session.Value.CreateImpl<PXCMCapture>(
            this.selectedCaptureModuleDescription.ImplDesc,
            out localCapture).Succeeded())
        {
          this.selectedCaptureModule = localCapture;
        }
      }
      base.OnPropertyChanged("DeviceDescriptions");
    }
    public StreamProfileViewModel SelectedStreamProfile
    {
      get
      {
        return (this.selectedStreamProfile);
      }
      set
      {
        base.SetProperty(ref this.selectedStreamProfile, value);
        this.SelectedStreamProfileChanged();
      }
    }
    async void SelectedStreamProfileChanged()
    {
      await this.StopStreamingAsync();

      if (this.selectedStreamProfile != null)
      {
        // TODO: Not sure I understand stream profile sets properly yet. What I find here is
        // that if I have a device that has used COLOR profile 1, DEPTH profile 2, IR
        // profile 3 then I can't change that device to use any other color profile or
        // depth profile. It's as though it gets 'stuck' on those profiles.
        // So...I'm destroying the device here 'just' so that I can have a chance
        // of changing its profile. I have tried the call to SetAllowProfileChange()
        // and I've tried ResetProperties() but nothing seems to work as of yet.
        if (this.currentDeviceNeedsRecreating)
        {
          this.RecreateDevice();
        }
        var result = this.selectedCaptureDevice.SetStreamProfileSet(
          this.selectedStreamProfile.StreamProfileSet);

        this.currentDeviceNeedsRecreating = true;

        if (result.Succeeded())
        {
          // fire and forget on this one.
          this.StartStreamingAsync();
        }
      }
    }
    async Task StartStreamingAsync()
    {
      try
      {
        this.streamingLoopCancelTokenSource = new CancellationTokenSource();
        this.streamingLoopTask = this.RunStreamingLoopAsync(this.streamingLoopCancelTokenSource.Token);
        await this.streamingLoopTask;
      }
      catch (OperationCanceledException)
      {
        // it's ok, we expect to be cancelled.
      }
    }
    async Task StopStreamingAsync()
    {
      if (this.streamingLoopCancelTokenSource != null)
      {
        this.streamingLoopCancelTokenSource.Cancel();

        try
        {
          // make sure it's stopped...
          await this.streamingLoopTask;
        }
        catch (OperationCanceledException)
        {
          this.streamingLoopCancelTokenSource.Dispose();
          this.streamingLoopCancelTokenSource = null;
          this.streamingLoopTask = null;
          this.ImageSource = null;
        }
      }
    }
    async Task RunStreamingLoopAsync(CancellationToken cancelToken)
    {
      // If we are streaming and the user changes (e.g.) the capture module or the device
      // then that will ripple binding changes such that at some point the SelectedStreamProfile
      // will be changed to NULL.
      // When that happens, the cancellation token passed here will be signalled and this task
      // will give up the ghost. However...because we're letting binding do the work, it means
      // that this function can't rely on class member variables that may get changed by
      // the 'ripple' that lasts until the streaming profile is changed - i.e. this function
      // will run on a little 'longer' than it theoretically should...
      ImageReader reader = new ImageReader(this.selectedCaptureDevice, this.selectedStreamProfile.StreamType);
      var streamType = this.selectedStreamProfile.StreamType;

      while (!cancelToken.IsCancellationRequested)
      {
        var sample = await reader.GetNextSampleAsync();

        if (sample != null)
        {
          PXCMImage image = sample.GetImageForStreamType(streamType);
          PXCMImage.ImageData imageData;

          if (image.AcquireAccess(
            PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out imageData).Succeeded())
          {
            this.EnsureImageSourceCreated(image);

            this.imageSource.WritePixels(
              this.imageDimensions,
              imageData.planes[0],
              this.imageSource.PixelWidth * this.imageSource.PixelHeight * 4,
              this.imageSource.PixelWidth * 4);

            image.ReleaseAccess(imageData);
          }
        }
      }
      cancelToken.ThrowIfCancellationRequested();
    }
    void RecreateDevice()
    {
      if (this.selectedCaptureDevice != null)
      {
        this.selectedCaptureDevice.Dispose();
      }
      this.selectedCaptureDevice =
        this.selectedCaptureModule.CreateDevice(this.selectedDeviceDescription.DeviceInfo.didx);

      this.currentDeviceNeedsRecreating = false;
    }
    void EnsureImageSourceCreated(PXCMImage image)
    {
      if (this.imageSource == null)
      {
        this.ImageSource = new WriteableBitmap(
          image.info.width,
          image.info.height,
          96,
          96,
          PixelFormats.Bgra32,
          null);

        this.imageDimensions = new Int32Rect(
          0, 0, image.info.width, image.info.height);
      }
    }
    CancellationTokenSource streamingLoopCancelTokenSource;
    Task streamingLoopTask;
    Int32Rect imageDimensions;
    WriteableBitmap imageSource;
    DeviceDescriptionViewModel selectedDeviceDescription;
    CaptureModuleViewModel selectedCaptureModuleDescription;
    StreamProfileViewModel selectedStreamProfile;
    PXCMCapture selectedCaptureModule;
    PXCMCapture.Device selectedCaptureDevice;
    Lazy<PXCMSession> session;
    bool currentDeviceNeedsRecreating;
  }
}

and the couple of points where this got ‘interesting’ for me are in;

  • the functions StartStreamingAsync, StopStreamingAsync, RunStreamingLoopAsync – these are ‘interesting’ for me because there are places where a simple click in the UI must stop any video streaming because the video module, device or streaming profile has been changed. That’s a bit challenging because the UI is data-bound which means an invocation onto a setter and a setter isn’t somewhere I can use ‘async/await’ style patterns and so I go to a bit of work here to try and make that work out.
  • the function RecreateDevice isn’t one that I originally planned to write but, so far, I’ve not figured out how to change the streaming profile on the device once it’s been set. More specifically, I find that once I’ve used the device for COLOR it will not accept any other COLOR profile but it will allow a new IR/DEPTH profile but, again, once one of those types of profiles have been used it seems I can’t switch. Consequently, at the moment the code destroys and recreates the PXCMCapture.Device instance in order to change profiles. I suspect this is a lack of understanding of how this is meant to work on my part.

I had a bit of fun putting that together and, in the end, it works ok in that I’ve a WPF app which supports displaying some of these streams.

( I should point out that the SDK already has samples that do this much better but I strongly believe that you learn stuff by doing it, not looking at samples Smile and, also, I don’t think there’s any WPF in the samples ).

If anyone (now or in the future) is playing with the SDK, I put the code for this post here for download