Kinect for Windows V2 SDK: Hello (Skeletal) World for the WPF Developer

Following on from my previous posts;

I thought I’d take some of the pieces that I’d just put together for that console demo and combine them with the pieces I’d put together for the video camera source demo and see if I could get WPF to draw a skeleton for me.

Once again, at the risk of repeating myself, I’m not trying to do anything “new” here and the samples in the SDK have much more than this functionality but I’m experimenting for myself and I figure I might as well write it up here as I go along. You can get the official view from the Channel 9 videos;

Programming-Kinect-for-Windows-v2

I took the “UI” that I’d made in the “Hello (Color) World” post above which is essentially just 4 buttons and an image and I replaced the image with a Canvas as below;

<Window x:Class="WpfApplication6.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>
    <Canvas x:Name="canvasDraw"
            HorizontalAlignment="Stretch"
            VerticalAlignment="Stretch"
            Background="Silver" />
    <StackPanel VerticalAlignment="Bottom"
                Orientation="Horizontal"
                HorizontalAlignment="Center">
      <StackPanel.Resources>
        <Style TargetType="Button">
          <Setter Property="Margin"
                  Value="5" />
        </Style>
      </StackPanel.Resources>
      <Button Content="Get Sensor"
              Click="OnGetSensor" />
      <Button Content="Open Reader"
              Click="OnOpenReader" />
      <Button Content="Close Reader"
              Click="OnCloseReader" />
      <Button Content="Close Sensor"
              Click="OnReleaseSensor" />
    </StackPanel>
  </Grid>
</Window>

and I then reworked some of the KinectControl class that I’ve been putting together in the last 2 posts such that it drew joints and skeletal connections using WPF rather than trying to draw using the console as in the last post.

I chose to try and draw in the simplest way that I know how in WPF which is to use a Canvas and then to add/remove shapes from it in response to frames coming off the sensor. This seems to work out fine and (hopefully) you can see it running smoothly in the video below;

In terms of the code changes that I made here, I did a bit of work on my IBodyDrawer interface because it was previously thinking in terms of things like ConsoleColor and that doesn’t sit well with WPF so I ended up with;

  interface IBodyDrawer
  {
    Brush Brush { get; set; }
    void Init(CoordinateMapper mapper, Int32Rect colourFrameSize);
    void ClearFrame();
    void DrawFrame(Body body);
  }

and the code behind my XAML page above is probably around 99% the same as the code that I had when I wrote the colour video blog post with the exception of using the KinectControl class that I’ve evolved since in a different way. That code-behind looks like;

using System;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Threading;

namespace WpfApplication6
{
  public partial class MainWindow : Window
  {
    KinectControl controller;

    public MainWindow()
    {
      InitializeComponent();
      this.Loaded += OnLoaded;
    }
    void OnLoaded(object sender, RoutedEventArgs e)
    {
      this.controller = new KinectControl(() => new CanvasBodyDrawer(this.canvasDraw));
    }
    void OnGetSensor(object sender, RoutedEventArgs e)
    {
      this.controller.GetSensor();
    }
    void OnOpenReader(object sender, RoutedEventArgs e)
    {
      this.controller.OpenReader();
    }
    void OnCloseReader(object sender, RoutedEventArgs e)
    {
      this.controller.CloseReader();
    }
    void OnReleaseSensor(object sender, RoutedEventArgs e)
    {
      this.controller.ReleaseSensor();
    }
  }
}

with the minor change being on line 21 which constructs the KinectControl class with a factory method which can produce an IBodyDrawer. The KinectControl class looks like;

using Microsoft.Kinect;
using System;
using System.Windows;
using System.Windows.Media;

namespace WpfApplication6
{
  class KinectControl
  {
    public KinectControl(Func<IBodyDrawer> bodyDrawerFactory)
    {
      this.bodyDrawerFactory = bodyDrawerFactory;
    }
    public void GetSensor()
    {
      this.sensor = KinectSensor.GetDefault();
      this.sensor.Open();

      Int32Rect colorFrameSize = new Int32Rect()
      {
        Width = this.sensor.ColorFrameSource.FrameDescription.Width,
        Height = this.sensor.ColorFrameSource.FrameDescription.Height
      };
      this.bodyDrawers = new IBodyDrawer[brushes.Length];

      for (int i = 0; i < brushes.Length; i++)
      {
        this.bodyDrawers[i] = bodyDrawerFactory();
        this.bodyDrawers[i].Brush = brushes[i];
        this.bodyDrawers[i].Init(this.sensor.CoordinateMapper, colorFrameSize);
      }
    }
    public void OpenReader()
    {
      this.reader = this.sensor.BodyFrameSource.OpenReader();
      this.reader.FrameArrived += OnFrameArrived;
    }
    public void CloseReader()
    {
      this.reader.FrameArrived -= OnFrameArrived;
      this.reader.Dispose();
      this.reader = null;
    }
    void OnFrameArrived(object sender, BodyFrameArrivedEventArgs e)
    {
      using (var frame = e.FrameReference.AcquireFrame())
      {
        if ((frame != null) && (frame.BodyCount > 0))
        {
          if ((this.bodies == null) || (this.bodies.Length != frame.BodyCount))
          {
            this.bodies = new Body[frame.BodyCount];
          }
          frame.GetAndRefreshBodyData(this.bodies);

          for (int i = 0; i < brushes.Length; i++)
          {           
            if (this.bodies[i].IsTracked)
            {
              this.bodyDrawers[i].DrawFrame(this.bodies[i]);
            }
            else
            {
              this.bodyDrawers[i].ClearFrame();
            }
          }
        }
      }
    }
    public void ReleaseSensor()
    {
      this.sensor.Close();
      this.sensor = null;
    }
    Body[] bodies;
    KinectSensor sensor;
    BodyFrameReader reader;
    IBodyDrawer[] bodyDrawers;
    Func<IBodyDrawer> bodyDrawerFactory;

    static Brush[] brushes = 
    {
      Brushes.Green,
      Brushes.Blue,
      Brushes.Red,
      Brushes.Orange,
      Brushes.Purple,
      Brushes.Yellow
    };
  }
}

and so is very similar to the one that I used in the console application with the differences being around how it initialises the IBodyDrawer implementation in the GetSensor() method and how it uses DrawFrame/ClearFrame slightly differently but I’d estimate around 80% of the code here is the same as I had in the console application – it’s getting hold of the Body array in the same way and checking for Tracked bodies and so on.

The remaining piece is the bit of code that I wrote to actually do the drawing. That’s represented by this CanvasBodyDrawer. The original idea of having an abstraction of a drawer hasn’t really worked out for me so well so far in that I keep changing the abstraction Smile but, regardless, this is another implementation of that idea for a WPF Canvas this time around;

using Microsoft.Kinect;
using System.Collections.Generic;
using System.Linq;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Shapes;

namespace WpfApplication6
{
  class CanvasBodyDrawer : IBodyDrawer
  {
    public CanvasBodyDrawer(Canvas canvas)
    {
      this.canvas = canvas;
      this.drawnJointElements = new Dictionary<JointType, Shape>();
      this.drawnLineElements = new List<Line>();
    }
    public Brush Brush
    {
      get;
      set;
    }
    public void Init(
      CoordinateMapper mapper,
      Int32Rect colourFrameSize)
    {
      this.canvasCoordMapper = new CanvasCoordMapper(this.canvas,
        mapper, colourFrameSize);
    }
    public void DrawFrame(Body body)
    {
      this.RemoveLinesFromPreviousFrame();

      var jointPositions = this.DrawJoints(body);

      this.DrawLines(jointPositions);
    }
    public void ClearFrame()
    {
      this.RemoveJointsFromPreviousFrame();
      this.RemoveLinesFromPreviousFrame();
    }
    void RemoveJointsFromPreviousFrame()
    {
      foreach (var item in this.drawnJointElements)
      {
        this.canvas.Children.Remove(item.Value);
      }
      this.drawnJointElements.Clear();
    }
    void RemoveLinesFromPreviousFrame()
    {
      foreach (var item in this.drawnLineElements)
      {
        this.canvas.Children.Remove(item);
      }
      this.drawnLineElements.Clear();
    }
    Dictionary<JointType, Point> DrawJoints(Body body)
    {
      Dictionary<JointType, Point> jointPositions = new Dictionary<JointType, Point>();

      foreach (var item in body.Joints)
      {
        JointType jointType = item.Key;
        Joint joint = item.Value;

        Point jointCanvasPosition = this.canvasCoordMapper.MapCameraSpacePoint(joint.Position);        

        bool draw = IsJointForDrawing(joint, jointCanvasPosition);

        Shape shape = null;

        if (draw && !this.drawnJointElements.TryGetValue(jointType, out shape))
        {
          shape = MakeShapeForJointType(jointType);
          this.drawnJointElements[jointType] = shape;
          this.canvas.Children.Add(shape);
        }
        if (draw)
        {
          shape.Fill =
            joint.TrackingState == TrackingState.Tracked ? this.Brush : InferredBrush;

          Canvas.SetLeft(shape, jointCanvasPosition.X - (shape.Width / 2));
          Canvas.SetTop(shape, jointCanvasPosition.Y - (shape.Height / 2));

          jointPositions[jointType] = jointCanvasPosition;
        }
        else if (shape != null)
        {
          this.canvas.Children.Remove(shape);
        }
      }
      return (jointPositions);
    }
    void DrawLines(IReadOnlyDictionary<JointType,Point> jointPositions)
    {
      foreach (var jointConnection in jointConnections)
      {
        // that little data structure either contains a list of joints to work through or
        // a start joint and an element count. it's discriminated and shouldn't contain
        // both!
        jointConnection.ForEachPair(
          (j1, j2) =>
          {
            if (jointPositions.ContainsKey(j1) && jointPositions.ContainsKey(j2))
            {
              Point p1 = jointPositions[j1];
              Point p2 = jointPositions[j2];
              Line line = MakeLineForPositions(p1, p2);
              this.canvas.Children.Add(line);
              this.drawnLineElements.Add(line);
            }
          }
        );
      }
    }
    static bool IsJointForDrawing(Joint joint, Point p)
    {
      return (
          (joint.TrackingState != TrackingState.NotTracked) &&
          (!double.IsInfinity(p.X)) &&
          (!double.IsInfinity(p.Y)));
    }
    static Line MakeLineForPositions(Point p1, Point p2)
    {
      return (new Line()
      {
        X1 = p1.X,
        Y1 = p1.Y,
        X2 = p2.X,
        Y2 = p2.Y,
        Stroke = LineBrush,
        StrokeThickness = 1
      });
    }
    static Shape MakeShapeForJointType(JointType jointType)
    { 
      JointType[] leafTypes =
      {
        JointType.Head,
        JointType.FootRight,
        JointType.FootLeft
      };
      bool large = leafTypes.Contains(jointType);
      int size = large ? LargeJointEllipseDiamater : JointEllipseDiameter;

      Shape element = new Ellipse()
      {
        Width = size,
        Height = size,
        VerticalAlignment = VerticalAlignment.Center,
        HorizontalAlignment = HorizontalAlignment.Center
      };
      return (element);
    }
    Dictionary<JointType, Shape> drawnJointElements;
    List<Line> drawnLineElements;
    Canvas canvas;
    CanvasCoordMapper canvasCoordMapper;

    static readonly Brush LineBrush = Brushes.Black;
    static readonly Brush InferredBrush = Brushes.LightGray;
    static readonly int JointEllipseDiameter = 10;
    static readonly int LargeJointEllipseDiamater = 30;

    // This is bad really because it depends on the actual enum values for the JointType
    // type in the SDK not changing.
    // It's easier though than having some massive array of all the connections but that 
    // would be the right thing to do I think.
    static JointConnection[] jointConnections = 
    {
      new JointConnection(JointType.SpineBase, 2),
      new JointConnection(JointType.ShoulderLeft, 4),
      new JointConnection(JointType.ShoulderRight, 4),
      new JointConnection(JointType.HipLeft, 4),
      new JointConnection(JointType.HipRight, 4),
      new JointConnection(JointType.Neck, 2),
      new JointConnection(JointType.SpineMid, JointType.SpineShoulder, JointType.Neck),
      new JointConnection(JointType.ShoulderLeft, JointType.SpineShoulder, JointType.ShoulderRight),
      new JointConnection(JointType.HipLeft, JointType.SpineBase, JointType.HipRight),
      new JointConnection(JointType.HandTipLeft, JointType.HandLeft),
      new JointConnection(JointType.HandTipRight, JointType.HandRight),
      new JointConnection(JointType.WristLeft, JointType.ThumbLeft),
      new JointConnection(JointType.WristRight, JointType.ThumbRight)
    };
  }
}

I’m not sure how (in)efficient this is going to be because it takes an approach of drawing a set of ellipses for the joints themselves and a separate pass to draw lines which connect them together. I could have tried to combine that into just a set of lines with particular end caps on those lines but this was my first attempt and the performance seems ok to date.

The approach taken involves drawing an ellipse at each joint position that is showing up as being tracked by the sensor and lines between the relevant connected joints.

The ellipses are drawn once and then moved around on subsequent frames (and removed if a previously tracked joint stops being tracked).

The lines are cleared and then drawn on every frame.

I don’t think that the Kinect SDK gives you any notion of a skeleton in the sense of which joint needs to be connected to which other joint and so at the bottom of that code file I have effectively built up my own skeleton and I’ve taken a shortcut in that I studied the values for the enumeration and figured out that certain ones are grouped together and I made use of that in the above code which as you know) will break if those enumerations moved around.

I wrote a tiny class to help me with that which is called JointConnection and which can either be constructed with a specific list of joint types or effectively fed a range of enumerations that live together (as on lines 175,176,177,etc above);

using Microsoft.Kinect;
using System;
using System.Linq;

namespace WpfApplication6
{
  class JointConnection
  {
    private JointConnection()
    {
    }
    public JointConnection(JointType startJoint, int jointCount)
    {
      this.joints =
         Enumerable
         .Range((int)startJoint, jointCount)
         .Select(j => (JointType)j)
         .ToArray();
    }
    public JointConnection(params JointType[] joints)
    {
      this.joints = joints;
    }
    public void ForEachPair(Action<JointType, JointType> handler)
    {
      for (int i = 0; i < this.joints.Length - 1; i++)
      {
        handler(this.joints[i], this.joints[i + 1]);
      }
    }
    JointType[] joints;
  }
}

There’s only one missing piece which is just a little helper class to help out with mapping between coordinates coming off the sensor and the coordinates understood by my Canvas;

using Microsoft.Kinect;
using System.Windows;
using System.Windows.Controls;

namespace WpfApplication6
{
  class CanvasCoordMapper
  {
    public CanvasCoordMapper(
      Canvas canvas,
      CoordinateMapper mapper,
      Int32Rect colourFrameSize)
    {
      this.canvas = canvas;
      this.mapper = mapper;
      this.colourFrameSize = colourFrameSize;
    }
    public Point MapCameraSpacePoint(CameraSpacePoint point)
    {
      double canvasWidth = this.canvas.ActualWidth;
      double canvasHeight = this.canvas.ActualHeight;

      var colourSpacePosition = this.mapper.MapCameraPointToColorSpace(point);

      return (new Point()
      {
        X = (colourSpacePosition.X / colourFrameSize.Width) * canvasWidth,
        Y = (colourSpacePosition.Y / colourFrameSize.Height) * canvasHeight
      });
    }
    Canvas canvas;
    CoordinateMapper mapper;
    Int32Rect colourFrameSize;
  }
}

and that’s all it took to go from the previous WPF code based around grabbing frames from the colour video camera and the previous console code based around grabbing skeletal data and to turn this into WPF code that’s displaying skeletal data.

What I’d really like to do with that next though is see if I can stop discarding the 3rd z-dimension off the data that I’ve got here and use WPF’s 3D capabilities to display this data. That may take some time…but I’m hoping that I won’t have to change the approach taken above too much to slot that in.

Meanwhile, the code for this post is here if anyone wants it for download.