Terminator Program: Part 1

July 8, 2014 Leave a comment

I am starting to work on a new Kinect application for TRINUG’s code camp.  I wanted to extend the facial recognition application I did using Sky Biometry and have the Kinect identify people in its field of view.  Then, I want to give the verbal command “Terminate XXX” where XXX is the name of a recognized person.  That would activate a couple of servos via a netduino and point a laser pointer at that person and perhaps make a blaster sound.  The <ahem> architectural diagram </ahem? looks like this

image

Not really worrying about how far I will get (the fun is in the process, no?), I picked up Rob Miles’s excellent book Start Here: Learn The Kinect API and plugged in my Kinect.

The first thing I did was see if I can get a running video from the Kinect –> which was very easy.  I created a new C#/WPF application and replaced the default markup with this::

  1. <Window x:Class="ChickenSoftware.Terminiator.UI.MainWindow"
  2.         xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation&quot;
  3.         xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml&quot;
  4.         Title="MainWindow" Height="545" Width="643"
  5.         Loaded="Window_Loaded" Closing="Window_Closing">
  6.     <Grid>
  7.         <Image x:Name="kinectColorImage" Width="640" Height="480" />
  8.     </Grid>
  9. </Window>

And in the code-behind, I added the following code.  The only thing that is kinda tricky is that there are two threads: the Main UI thread and then the thread that processes the Kinect data.  Interestingly, it is easy to pass data from the Kinect Thread to the Main UI Thread –> just call the delegate and pass in the byte array.

  1. Boolean _isKinectDisplayActive = false;
  2. KinectSensor _sensor = null;
  3. WriteableBitmap _videoBitmap = null;
  4.  
  5. private void Window_Loaded(object sender, RoutedEventArgs e)
  6. {
  7.     SetUpKinect();
  8.     Thread videoThread = new Thread(new ThreadStart(DisplayKinectData));
  9.     _isKinectDisplayActive = true;
  10.     videoThread.Start();
  11. }
  13. private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
  14. {
  15.     _isKinectDisplayActive = false;
  16.  
  17. }
  18.  
  19. private void SetUpKinect()
  20. {
  21.     _sensor = KinectSensor.KinectSensors[0];
  22.     _sensor.ColorStream.Enable();
  23.     _sensor.Start();
  24. }
  25.  
  26. private void DisplayKinectData()
  27. {
  28.     while (_isKinectDisplayActive)
  29.     {
  30.         using (ColorImageFrame colorFrame = _sensor.ColorStream.OpenNextFrame(10))
  31.         {
  32.             if (colorFrame == null) continue;
  33.             var colorData = new byte[colorFrame.PixelDataLength];
  34.             colorFrame.CopyPixelDataTo(colorData);
  35.             Dispatcher.Invoke(new Action(() => UpdateDisplay(colorData)));
  36.         }
  37.     }
  38.     _sensor.Stop();
  39. }
  40.  
  41. private void UpdateDisplay(byte[] colorData)
  42. {
  43.     if (_videoBitmap == null)
  44.     {
  45.         _videoBitmap = new WriteableBitmap(640, 480, 96, 96, PixelFormats.Bgr32, null);
  46.     }
  47.     _videoBitmap.WritePixels(new Int32Rect(0, 0, 640, 480), colorData, 640 * 4, 0);
  48.     kinectColorImage.Source = _videoBitmap;
  49. }

And I have a live-feed video

image

With that out of the way, I went to add picture taking capability.  I altered the XAML like so:

  1. <Window x:Class="ChickenSoftware.Terminiator.UI.MainWindow"
  2.         xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation&quot;
  3.         xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml&quot;
  4.         Title="MainWindow" Height="545" Width="643"
  5.         Loaded="Window_Loaded" Closing="Window_Closing">
  6.     <Grid>
  7.         <Image x:Name="kinectColorImage" Width="640" Height="480" />
  8.         <Button x:Name="takePhotoButton" Margin="0,466,435,10" Click="takePhotoButton_Click">Take Photo</Button>
  9.     </Grid>
  10. </Window>

And added this to the code behind:

  1. Boolean _isTakingPicture = false;
  2. BitmapSource _pictureBitmap = null;
  3.  
  4. private void takePhotoButton_Click(object sender, RoutedEventArgs e)
  5. {
  6.     _isTakingPicture = true;
  7.     SaveFileDialog dialog = new SaveFileDialog();
  8.     dialog.FileName = "Snapshot";
  9.     dialog.DefaultExt = ".jpg";
  10.     dialog.Filter = "Pictures (.jpg)|*.jpg";
  11.  
  12.     if (dialog.ShowDialog() == true)
  13.     {
  14.         String fileName = dialog.FileName;
  15.         using (FileStream fileStream = new FileStream(fileName, FileMode.Create))
  16.         {
  17.             JpegBitmapEncoder encoder = new JpegBitmapEncoder();
  18.             encoder.Frames.Add(BitmapFrame.Create(_pictureBitmap));
  19.             encoder.Save(fileStream);
  20.         }
  21.     }
  22. }

 

And altered the DisplayKinectDatra method to poll the _isTakingPicture flag

  1. private void DisplayKinectData()
  2. {
  3.     while (_isKinectDisplayActive)
  4.     {
  5.         using (ColorImageFrame colorFrame = _sensor.ColorStream.OpenNextFrame(10))
  6.         {
  7.             if (colorFrame == null) continue;
  8.             var colorData = new byte[colorFrame.PixelDataLength];
  9.             colorFrame.CopyPixelDataTo(colorData);
  10.             Dispatcher.Invoke(new Action(() => UpdateDisplay(colorData)));
  11.  
  12.             if (_isTakingPicture)
  13.             {
  14.                 Dispatcher.Invoke(new Action(() => SavePhoto(colorData)));
  15.             }
  16.         }
  17.     }
  18.     _sensor.Stop();
  19. }

And now I have screen capture ability.

image

With that out of the way, I needed a way of identifying the people in the Kinect’s field of vision and taking their picture individually.  I altered the XAML like so

  1. <Window x:Class="ChickenSoftware.Terminiator.UI.MainWindow"
  2.         xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation&quot;
  3.         xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml&quot;
  4.         Title="MainWindow" Height="545" Width="643"
  5.         Loaded="Window_Loaded" Closing="Window_Closing">
  6.     <Grid>
  7.         <Image x:Name="kinectColorImage" Width="640" Height="480" />
  8.         <Button x:Name="takePhotoButton" Margin="0,466,435,10" Click="takePhotoButton_Click">Take Photo</Button>
  9.         <Canvas x:Name="skeletonCanvas" Width="640" Height="480" />
  10.                 <TextBox x:Name="skeletonInfoTextBox" Margin="205,466,10,10" />
  11.     </Grid>
  12. </Window>

And altered the Setup method like so:

  1. private void SetUpKinect()
  2. {
  3.     _sensor = KinectSensor.KinectSensors[0];
  4.     _sensor.ColorStream.Enable();
  5.     _sensor.SkeletonStream.Enable();
  6.     _sensor.Start();
  7. }

And then altered the UpdateDisplay method to take in both the color byte array and the skeleton byte array and display the head and skeleton location.  Note that there is a built in function called MapSkeletonPointToColorPoint() which takes the skeleton coordinate position and translates it to the color coordinate position.  I know that is needed, but I have no idea who it works –> magic I guess.

  1. private void UpdateDisplay(byte[] colorData, Skeleton[] skeletons)
  2. {
  3.     if (_videoBitmap == null)
  4.     {
  5.         _videoBitmap = new WriteableBitmap(640, 480, 96, 96, PixelFormats.Bgr32, null);
  6.     }
  7.     _videoBitmap.WritePixels(new Int32Rect(0, 0, 640, 480), colorData, 640 * 4, 0);
  8.     kinectColorImage.Source = _videoBitmap;
  9.     var selectedSkeleton = skeletons.FirstOrDefault(s => s.TrackingState == SkeletonTrackingState.Tracked);
  10.     if (selectedSkeleton != null)
  11.     {
  12.         var headPosition = selectedSkeleton.Joints[JointType.Head].Position;
  13.         var adjustedHeadPosition =
  14.             _sensor.CoordinateMapper.MapSkeletonPointToColorPoint(headPosition, ColorImageFormat.RgbResolution640x480Fps30);
  15.         var adjustedSkeletonPosition = _sensor.CoordinateMapper.MapSkeletonPointToColorPoint(selectedSkeleton.Position, ColorImageFormat.RgbResolution640x480Fps30);
  16.  
  17.  
  18.         String skeletonInfo = headPosition.X.ToString() + " : " + headPosition.Y.ToString() + " — ";
  19.         skeletonInfo = skeletonInfo + adjustedHeadPosition.X.ToString() + " : " + adjustedHeadPosition.Y.ToString() + " — ";
  20.         skeletonInfo = skeletonInfo + adjustedSkeletonPosition.X.ToString() + " : " + adjustedSkeletonPosition.Y.ToString();
  21.  
  22.         skeletonInfoTextBox.Text = skeletonInfo;
  23.  
  24.     }
  25. }

And the invocation of the UpdateDisplay now looks like this:

  1. private void DisplayKinectData()
  2. {
  3.     while (_isKinectDisplayActive)
  4.     {
  5.         using (ColorImageFrame colorFrame = _sensor.ColorStream.OpenNextFrame(10))
  6.         {
  7.             if (colorFrame == null) continue;
  8.             using (SkeletonFrame skeletonFrame = _sensor.SkeletonStream.OpenNextFrame(10))
  9.             {
  10.                 if (skeletonFrame == null) continue;
  11.  
  12.                 var colorData = new byte[colorFrame.PixelDataLength];
  13.                 var skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
  14.  
  15.                 colorFrame.CopyPixelDataTo(colorData);
  16.                 skeletonFrame.CopySkeletonDataTo(skeletons);
  17.  
  18.  
  19.                 if (_isTakingPicture)
  20.                 {
  21.                     Dispatcher.Invoke(new Action(() => SavePhoto(colorData)));
  22.                 }
  23.                 Dispatcher.Invoke(new Action(() => UpdateDisplay(colorData, skeletons)));
  24.  
  25.             }
  26.         }
  27.     }
  28.     _sensor.Stop();
  29. }

And the results are what you expect:

image

With the ability to identify individuals, I then wants to take individual photos of each person and feed it to Sky Biometry.  To that end, I added a method to draw a rectangle around each person and then (somehow) take a snapshot of the contents within the triangle.  Drawing the rectangle was a straight-forward WPF exercise:

  1. private void DrawBoxAroundHead(Skeleton selectedSkeleton)
  2. {
  3.     skeletonCanvas.Children.Clear();
  4.     var headPosition = selectedSkeleton.Joints[JointType.Head].Position;
  5.     var shoulderCenterPosition = selectedSkeleton.Joints[JointType.ShoulderCenter].Position;
  6.  
  7.     var adjustedHeadPosition =
  8.         _sensor.CoordinateMapper.MapSkeletonPointToColorPoint(headPosition, ColorImageFormat.RgbResolution640x480Fps30);
  9.     var adjustedShoulderCenterPosition =
  10.         _sensor.CoordinateMapper.MapSkeletonPointToColorPoint(shoulderCenterPosition, ColorImageFormat.RgbResolution640x480Fps30);
  11.     var delta = adjustedHeadPosition.Y – adjustedShoulderCenterPosition.Y;
  12.     var centerX = adjustedHeadPosition.X;
  13.     var centerY = adjustedHeadPosition.Y;
  14.  
  15.     Line topLline = new Line();
  16.     topLline.Stroke = new SolidColorBrush(Colors.Red);
  17.     topLline.StrokeThickness = 5;
  18.     topLline.X1 = centerX + (delta * -1);
  19.     topLline.Y1 = centerY – (delta * -1);
  20.     topLline.X2 = centerX + delta;
  21.     topLline.Y2 = centerY – (delta * -1);
  22.     skeletonCanvas.Children.Add(topLline);
  23.     Line bottomLine = new Line();
  24.     bottomLine.Stroke = new SolidColorBrush(Colors.Red);
  25.     bottomLine.StrokeThickness = 5;
  26.     bottomLine.X1 = centerX + (delta * -1);
  27.     bottomLine.Y1 = centerY + (delta * -1);
  28.     bottomLine.X2 = centerX + delta;
  29.     bottomLine.Y2 = centerY + (delta * -1);
  30.     skeletonCanvas.Children.Add(bottomLine);
  31.     Line rightLine = new Line();
  32.     rightLine.Stroke = new SolidColorBrush(Colors.Red);
  33.     rightLine.StrokeThickness = 5;
  34.     rightLine.X1 = centerX + (delta * -1);
  35.     rightLine.Y1 = centerY – (delta * -1);
  36.     rightLine.X2 = centerX + (delta * -1);
  37.     rightLine.Y2 = centerY + (delta * -1);
  38.     skeletonCanvas.Children.Add(rightLine);
  39.     Line leftLine = new Line();
  40.     leftLine.Stroke = new SolidColorBrush(Colors.Red);
  41.     leftLine.StrokeThickness = 5;
  42.     leftLine.X1 = centerX + delta;
  43.     leftLine.Y1 = centerY – (delta * -1);
  44.     leftLine.X2 = centerX + delta;
  45.     leftLine.Y2 = centerY + (delta * -1);
  46.     skeletonCanvas.Children.Add(leftLine);
  47. }

And then adding that line in the Update Display

  1. if (selectedSkeleton != null)
  2. {
  3.     var headPosition = selectedSkeleton.Joints[JointType.Head].Position;
  4.     var adjustedHeadPosition =
  5.         _sensor.CoordinateMapper.MapSkeletonPointToColorPoint(headPosition, ColorImageFormat.RgbResolution640x480Fps30);
  6.     var adjustedSkeletonPosition = _sensor.CoordinateMapper.MapSkeletonPointToColorPoint(selectedSkeleton.Position, ColorImageFormat.RgbResolution640x480Fps30);
  7.  
  8.     DrawBoxAroundHead(selectedSkeleton);
  9.  
  10.     String skeletonInfo = headPosition.X.ToString() + " : " + headPosition.Y.ToString() + " — ";
  11.     skeletonInfo = skeletonInfo + adjustedHeadPosition.X.ToString() + " : " + adjustedHeadPosition.Y.ToString() + " — ";
  12.     skeletonInfo = skeletonInfo + adjustedSkeletonPosition.X.ToString() + " : " + adjustedSkeletonPosition.Y.ToString();
  13.  
  14.     skeletonInfoTextBox.Text = skeletonInfo;
  15.  
  16. }

Gives me this:

image

Which is great, but now I am stuck.  I need a way of isolating the contents of that rectangle in the byte array that I am feeding to bitmap encoder and I don’t know how to trim the array.  Instead of trying to learn any more WPF and graphic programming, I decided to take a different tact and send the photograph in its entirety to Sky Biometry and let it figure out the people in the photograph.  How I did that is the subject of my next blog post…

 

 

 

 

Filed under C#, Kinect, WPF

Leave a Reply

Fill in your details below or click an icon to log in:

Gravatar
WordPress.com Logo

You are commenting using your WordPress.com account. ( Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. ( Log Out /  Change )

Cancel

Connecting to %s

%d bloggers like this: