ページ

Wednesday, February 23, 2011

Kinect : Skeleton Tracking

Kinect seems to become hot topic since the end of last year.
So I tried using Kinect and programming for skeleton tracking.


* Note : 
    OpenNI : http://www.openni.org/
    Prime Sensehttp://www.primesense.com/
    SensorKinect : https://github.com/avin2/SensorKinect 
    XNA 4.0 : http://msdn.microsoft.com/en-En/xna/default.aspx




TrackingConfig.xml

<OpenNI>
 <Licenses>
  <License vendor="PrimeSense" key="0KOIk2JeIBYClPWVnMoRKn5cdY4="/>
 </Licenses>
 <Log writeToConsole="true" writeToFile="false">
  <!-- 0 - Verbose, 1 - Info, 2 - Warning, 3 - Error (default) -->
  <LogLevel value="3"/>
  <Masks>
   <Mask name="ALL" on="false"/>
  </Masks>
  <Dumps>
  </Dumps>
 </Log>
 <ProductionNodes>
    <Node type="Image" name="Image1">
      <Configuration>
        <MapOutputMode xRes="640" yRes="480" FPS="30"/>
        <Mirror on="true"/>
      </Configuration>
    </Node>
    <Node type="Depth">
   <Configuration>
    <MapOutputMode xRes="640" yRes="480" FPS="30"/>
    <Mirror on="true"/>
   </Configuration>
  </Node>
  <Node type="Gesture" />
  <Node type="Hands" />
 </ProductionNodes>
</OpenNI>

MainWindow.xaml

<Window x:Class="KinectWPF.MainWindow"
        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
        Title="Kinect Demo" Height="960" Width="640"
        xmlns:kinect="clr-namespace:KinectWPF">
    <Grid Background="White">
 <Grid.RowDefinitions>
  <RowDefinition Height="*" />
  <RowDefinition Height="*" />
 </Grid.RowDefinitions>
        <Image Name="imgDepth" Width="640" Height="480" />
        <WindowsFormsHost Background="Transparent" Name="windowsFormsHost1" Width="640" Height="480" Grid.Row="1">
            <kinect:KinectCustomControl x:Name="KinectCustomControl" />
        </WindowsFormsHost>
    </Grid>
</Window>


MainWindow.xaml.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;

using xn;

using System.ComponentModel;
using System.Threading;

namespace KinectWPF
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        private const string CONFIGURATION = @"TrackingConfig.xml";
        private readonly int DPI_X = 96;
        private readonly int DPI_Y = 96;
        private Thread _thread;
        private bool _isRunning = true;

        public Context _context;
        public ImageGenerator _image;
        public DepthGenerator _depth;

        private WriteableBitmap _imageBmp;
        private WriteableBitmap _depthBmp;
        private ImageMetaData _imgMD = new ImageMetaData();
        private DepthMetaData _depthMD = new DepthMetaData();

        public int[] _histogram;

        BackgroundWorker _worker = new BackgroundWorker();

        public ImageSource RawImageSource
        {
            get
            {
                if (_imageBmp != null)
                {
                    _imageBmp.Lock();
                    _imageBmp.WritePixels(new Int32Rect(0, 0, _imgMD.XRes, _imgMD.YRes), _imgMD.ImageMapPtr, (int)_imgMD.DataSize, _imageBmp.BackBufferStride);
                    _imageBmp.Unlock();
                }

                return _imageBmp;
            }
        }

        public ImageSource DepthImageSource
        {
            get
            {
                if (_depthBmp != null)
                {
                    UpdateHistogram(_depthMD);

                    _depthBmp.Lock();

                    unsafe
                    {
                        ushort* pDepth = (ushort*)_depth.GetDepthMapPtr().ToPointer();
                        for (int y = 0; y < _depthMD.YRes; ++y)
                        {
                            byte* pDest = (byte*)_depthBmp.BackBuffer.ToPointer() + y * _depthBmp.BackBufferStride;
                            for (int x = 0; x < _depthMD.XRes; ++x, ++pDepth, pDest += 3)
                            {
                                byte pixel = (byte)_histogram[*pDepth];
                                pDest[0] = 0;
                                pDest[1] = pixel;
                                pDest[2] = pixel;
                            }
                        }
                    }

                    _depthBmp.AddDirtyRect(new Int32Rect(0, 0, _depthMD.XRes, _depthMD.YRes));
                    _depthBmp.Unlock();
                }

                return _depthBmp;
            }
        }

        public MainWindow()
        {
            InitializeComponent();
            CompositionTarget.Rendering += new EventHandler(CompositionTarget_Rendering);
            this.Closing += new CancelEventHandler(Window_Closing);

            InitializeKinect(CONFIGURATION);
            InitializeBitmaps();
            InitializeThread();
        }

        private void InitializeKinect(string configuration)
        {
            _context = new Context(configuration);
            if (_context == null)
                throw new Exception("configuration file is not found.");

            _image = _context.FindExistingNode(NodeType.Image) as ImageGenerator;
            if (_image == null)
                throw new Exception("Viewer must have a image node!");

            _depth = _context.FindExistingNode(NodeType.Depth) as DepthGenerator;
            if (_depth == null)
                throw new Exception("Viewer must have a depth node!");

            _histogram = new int[_depth.GetDeviceMaxDepth()];
        }

        private void InitializeBitmaps()
        {
            MapOutputMode mapMode = _depth.GetMapOutputMode();

            int width = (int)mapMode.nXRes;
            int height = (int)mapMode.nYRes;

            _imageBmp = new WriteableBitmap(width, height, DPI_X, DPI_Y, PixelFormats.Rgb24, null);
            _depthBmp = new WriteableBitmap(width, height, DPI_X, DPI_Y, PixelFormats.Rgb24, null);
        }

        private void InitializeThread()
        {
            _worker.DoWork += new DoWorkEventHandler(Worker_DoWork);
            _thread = new Thread(CameraThread);
            _thread.IsBackground = true;
            _isRunning = true;
            _thread.Start();
        }

        private unsafe void CameraThread()
        {
            while (_isRunning)
            {
                _context.WaitAndUpdateAll();

                _image.GetMetaData(_imgMD);
                _depth.GetMetaData(_depthMD);
            }
        }

        public unsafe void UpdateHistogram(DepthMetaData depthMD)
        {
            for (int i = 0; i < _histogram.Length; ++i)
                _histogram[i] = 0;

            ushort* pDepth = (ushort*)depthMD.DepthMapPtr.ToPointer();

            int points = 0;
            for (int y = 0; y < depthMD.YRes; ++y)
            {
                for (int x = 0; x < depthMD.XRes; ++x, ++pDepth)
                {
                    ushort depthVal = *pDepth;
                    if (depthVal != 0)
                    {
                        _histogram[depthVal]++;
                        points++;
                    }
                }
            }

            for (int i = 1; i < _histogram.Length; i++)
            {
                _histogram[i] += _histogram[i - 1];
            }

            if (points > 0)
            {
                for (int i = 1; i < _histogram.Length; i++)
                {
                    _histogram[i] = (int)(256 * (1.0f - (_histogram[i] / (float)points)));
                }
            }
        }

        private void Dispose()
        {
            _imageBmp = null;
            _depthBmp = null;
            _isRunning = false;
            if (_thread != null)
            {
                _thread.Join();
                _thread = null;
            }
            if (_worker != null)
            {
                _worker.Dispose();
                _worker = null;
            }
            if (_depth != null)
            {
                _depth.Dispose();
                _depth = null;
            }
            if (_image != null)
            {
                _image.Dispose();
                _image = null;
            }
            if (_context != null)
            {
                _context.Dispose();
                _context = null;
            }
        }

        private void Worker_DoWork(object sender, DoWorkEventArgs e)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                imgDepth.Source = DepthImageSource;
            });
        }

        private void Window_Closing(object sender, CancelEventArgs e)
        {
            this.Dispose();
        }

        private void CompositionTarget_Rendering(object sender, EventArgs e)
        {
            if (!_worker.IsBusy)
            {
                _worker.RunWorkerAsync();
            }
        }
    }
}


KinectCustomControl.cs

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

using xn;

using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Color = Microsoft.Xna.Framework.Color;

namespace KinectWPF
{
    public partial class KinectCustomControl : Control
    {
        private const string CONFIGURATION = @"TrackingConfig.xml";
        private GraphicsDevice _device = null;
        private PresentationParameters _pp = null;
        private BasicEffect _effect = null;
        private List<VertexPositionColor> _vertices;
        
        private System.Windows.Forms.Timer _timer;

        Context _context;
        DepthGenerator _depth;
        UserGenerator _users;
        SkeletonCapability _skeleton;
        PoseDetectionCapability _pose;
        Dictionary<uint, Dictionary<SkeletonJoint, SkeletonJointPosition>> _joints;

        private readonly float _nearPlaneDistance = 1.0f;
        private readonly float _farPlaneDistance = 2000.0f;

        public KinectCustomControl()
        {
            InitializeComponent();
        }

        protected override void OnCreateControl()
        {
            if (this.DesignMode == false)
            {
                InitializeKinect(CONFIGURATION);
                InitializeXnaFramework();
                InitializeUserGenerator();
                InitializePoseDetectionCapability();
                InitializeSkeletonCapability(SkeletonProfile.Upper);
                _users.StartGenerating();
            }
            base.OnCreateControl();
        }

        private void InitializeXnaFramework()
        {
            _vertices = new List<VertexPositionColor>();
            try
            {
                _pp = new PresentationParameters();
                _pp.BackBufferWidth = 640;
                _pp.BackBufferHeight = 480;
                _pp.BackBufferFormat = SurfaceFormat.Color;
                _pp.DeviceWindowHandle = this.Handle;
                _pp.DepthStencilFormat = DepthFormat.Depth16;
                _pp.IsFullScreen = false;
                _device = new GraphicsDevice(GraphicsAdapter.DefaultAdapter, GraphicsProfile.Reach, _pp);

                _effect = new BasicEffect(_device);
                _effect.VertexColorEnabled = true;
                _effect.View = Matrix.CreateLookAt(
                        new Vector3(0.0f, 0.0f, -1000.0f),
                        Vector3.Zero,
                        Vector3.Down);

                Matrix world = Matrix.Identity;
                world *= Matrix.CreateTranslation(new Vector3(-_device.Viewport.Width/2.0f, -_device.Viewport.Height/2.0f, -500.0f));
                world *= Matrix.CreateScale(1.0f, 1.0f, 1.0f);
                _effect.World = world;
                _effect.Projection = Matrix.CreatePerspectiveFieldOfView(
                    MathHelper.ToRadians(45.0f), _device.Viewport.AspectRatio, _nearPlaneDistance, _farPlaneDistance);
            }
            catch (Exception ex)
            {
                System.Diagnostics.Trace.WriteLine(ex.ToString());
            }
        }

        private void InitializeKinect(string config)
        {
            _context = new Context(config);
            if(_context==null)
                throw new Exception("configuration file is not found.");

            _depth = _context.FindExistingNode(NodeType.Depth) as DepthGenerator;
            if (_depth == null)
                throw new Exception("Viewer must have a depth node!");
        }

        private void InitializeUserGenerator()
        {
            _users = new UserGenerator(_context);
            _users.NewUser += new UserGenerator.NewUserHandler(UserGesture_NewUser);
            _users.LostUser += new UserGenerator.LostUserHandler(UserGesture_LostUser);
        }

        private void InitializePoseDetectionCapability()
        {
            _pose = new PoseDetectionCapability(_users);
            _pose.PoseDetected += new PoseDetectionCapability.PoseDetectedHandler(PoseDetectionCapability_PoseDetected);
        }

        private void InitializeSkeletonCapability(SkeletonProfile profile)
        {
            _skeleton = new SkeletonCapability(_users);
            _skeleton.CalibrationEnd += new SkeletonCapability.CalibrationEndHandler(SkeletonCapability_CalibrationEnd);
            _skeleton.SetSkeletonProfile(profile);
            _joints = new Dictionary<uint, Dictionary<SkeletonJoint, SkeletonJointPosition>>();
        }

        private void SkeletonCapability_CalibrationEnd(ProductionNode node, uint id, bool success)
        {
            if (success)
            {
                _skeleton.StartTracking(id);
                _joints.Add(id, new Dictionary<SkeletonJoint, SkeletonJointPosition>());
            }
            else
            {
                _pose.StartPoseDetection(_skeleton.GetCalibrationPose(), id);
            }
        }

        private void PoseDetectionCapability_PoseDetected(ProductionNode node, string pose, uint id)
        {
            _pose.StopPoseDetection(id);
            _skeleton.RequestCalibration(id, true);
        }

        private void UserGesture_LostUser(ProductionNode node, uint id)
        {
            _joints.Remove(id);
        }

        private void UserGesture_NewUser(ProductionNode node, uint id)
        {
            _pose.StartPoseDetection(_skeleton.GetCalibrationPose(), id);
        }

        private void SetUserSkeletonJointPositions(uint user)
        {
            SetSkeletonJointPosition(user, SkeletonJoint.Head);
            SetSkeletonJointPosition(user, SkeletonJoint.LeftHand);
            SetSkeletonJointPosition(user, SkeletonJoint.RightHand);
            SetSkeletonJointPosition(user, SkeletonJoint.Neck);

            SetSkeletonJointPosition(user, SkeletonJoint.LeftShoulder);
            SetSkeletonJointPosition(user, SkeletonJoint.LeftElbow);

            SetSkeletonJointPosition(user, SkeletonJoint.RightShoulder);
            SetSkeletonJointPosition(user, SkeletonJoint.RightElbow);

            SetSkeletonJointPosition(user, SkeletonJoint.Torso);


            // Uncomment When the SkeletonProfile is set as All ( or Lower )
            /*
            GetSkeletonJointPosition(user, SkeletonJoint.LeftHip);
            GetSkeletonJointPosition(user, SkeletonJoint.LeftKnee);
            GetSkeletonJointPosition(user, SkeletonJoint.LeftFoot);

            GetSkeletonJointPosition(user, SkeletonJoint.RightHip);
            GetSkeletonJointPosition(user, SkeletonJoint.RightKnee);
            GetSkeletonJointPosition(user, SkeletonJoint.RightFoot);
            */
        }

        private void SetSkeletonJointPosition(uint user, SkeletonJoint joint)
        {
            try
            {
                SkeletonJointPosition pos = new SkeletonJointPosition();
                _skeleton.GetSkeletonJointPosition(user, joint, ref pos);
                if (pos.position.Z == 0)
                    pos.fConfidence = 0;
                else
                    pos.position = _depth.ConvertRealWorldToProjective(pos.position);
                _joints[user][joint] = pos;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Trace.WriteLine(ex.ToString());
            }
        }

        protected override void OnPaint(PaintEventArgs pe)
        {
            base.OnPaint(pe);
        }

        protected override void OnPaintBackground(PaintEventArgs pevent)
        {
            //base.OnPaintBackground(pevent);
        }

        private void DrawSkeleton(Color color, uint user)
        {
            SetUserSkeletonJointPositions(user);
            Dictionary<SkeletonJoint, SkeletonJointPosition> dict = _joints[user];

            SetVertexPositionColors(dict, SkeletonJoint.Head, SkeletonJoint.Neck, color);
            SetVertexPositionColors(dict, SkeletonJoint.Neck, SkeletonJoint.RightShoulder, color);
            SetVertexPositionColors(dict, SkeletonJoint.Neck, SkeletonJoint.LeftShoulder, color);

            SetVertexPositionColors(dict, SkeletonJoint.RightShoulder, SkeletonJoint.RightElbow, color);
            SetVertexPositionColors(dict, SkeletonJoint.RightElbow, SkeletonJoint.RightHand, color);

            SetVertexPositionColors(dict, SkeletonJoint.LeftShoulder, SkeletonJoint.LeftElbow, color);
            SetVertexPositionColors(dict, SkeletonJoint.LeftElbow, SkeletonJoint.LeftHand, color);

            SetVertexPositionColors(dict, SkeletonJoint.RightHip, SkeletonJoint.Torso, color);
            SetVertexPositionColors(dict, SkeletonJoint.LeftHip, SkeletonJoint.Torso, color);
            SetVertexPositionColors(dict, SkeletonJoint.RightHip, SkeletonJoint.LeftHip, color);

            SetVertexPositionColors(dict, SkeletonJoint.RightHip, SkeletonJoint.RightKnee, color);
            SetVertexPositionColors(dict, SkeletonJoint.RightKnee, SkeletonJoint.RightFoot, color);

            SetVertexPositionColors(dict, SkeletonJoint.LeftHip, SkeletonJoint.LeftKnee, color);
            SetVertexPositionColors(dict, SkeletonJoint.LeftKnee, SkeletonJoint.LeftFoot, color);

            if (_vertices.Count == 0)
                return;
            VertexPositionColor[] vertexis = new VertexPositionColor[_vertices.Count()];
            for (int i = 0; i < _vertices.Count(); i++)
            {
                vertexis[i] = _vertices[i];
            }
            _device.DrawUserPrimitives(PrimitiveType.LineList, vertexis, 0, vertexis.Length / 2);
        }

        private void Timer_Tick(object sender, EventArgs e)
        {
            SkeletonThread();
        }

        private void  SetVertexPositionColors(Dictionary<SkeletonJoint, SkeletonJointPosition> joints, SkeletonJoint joint0, SkeletonJoint joint1, Color color)
        {
            if (!joints.ContainsKey(joint0) || !joints.ContainsKey(joint1))
                return ;

            try
            {
                SkeletonJointPosition p1 = joints[joint0];
                SkeletonJointPosition p2 = joints[joint1];
                Vector3 v1 = new Vector3(p1.position.X, p1.position.Y, p1.position.Z);
                Vector3 v2 = new Vector3(p2.position.X, p2.position.Y, p2.position.Z);
                _vertices.Add(new VertexPositionColor(v1, color));
                _vertices.Add(new VertexPositionColor(v2, color));
            }
            catch (Exception ex)
            {
                System.Diagnostics.Trace.WriteLine(ex.ToString());
            }
        }

        private unsafe void SkeletonThread()
        {
            DepthMetaData depthMD = new DepthMetaData();
            try
            {
                _context.WaitAndUpdateAll();
            }
            catch (Exception ex)
            {
                System.Diagnostics.Trace.WriteLine(ex.ToString());
            }
            _depth.GetMetaData(depthMD);

            lock (this)
            {
                if (_device == null)
                    return;

                _device.Clear(Color.White);
                _effect.Techniques[0].Passes[0].Apply();

                uint[] users = _users.GetUsers();
                foreach (uint user in users)
                {
                    if (_skeleton.IsTracking(user))
                    {
                        // Tracking
                        _vertices.Clear();
                        DrawSkeleton(Color.Red, user);
                    }
                    else if (_skeleton.IsCalibrating(user))
                    {
                        // Calibrating
                    }
                    else
                    {
                        // Looking for pose or something else.
                    }
                }
                try
                {
                    _device.Present();
                }
                catch (Exception ex)
                {
                    System.Diagnostics.Trace.WriteLine(ex.ToString());
                }
            }
        }


    }
}


KinectCustomControl.Designer.cs

namespace KinectWPF
{
    partial class KinectCustomControl
    {
        /// <summary>
        /// Required designer variable.
        /// </summary>
        private System.ComponentModel.IContainer components = null;
        /// <summary>
        /// Clean up any resources being used.
        /// </summary>
        /// <param name="disposing">true if managed resources should be disposed; otherwise, false.</param>
        protected override void Dispose(bool disposing)
        {
            if (disposing && (components != null))
            {
                components.Dispose();
            }
            if (_timer != null)
            {
                _timer.Dispose();
                _timer = null;
            }
            if (_skeleton != null)
            {
                _skeleton.Dispose();
                _skeleton = null;
            }
            if (_pose != null)
            {
                _pose.Dispose();
                _pose = null;
            }
            if (_users != null)
            {
                _users.Dispose();
                _users = null;
            }
            if (_depth != null)
            {
                _depth.Dispose();
                _depth = null;
            }
            if (_context != null)
            {
                _context.Dispose();
                _context = null;
            }
            if (_effect != null)
            {
                _effect.Dispose();
                _effect = null;
            }
            if (_device != null)
            {
                _device.Dispose();
                _device = null;
            }
            base.Dispose(disposing);
        }

        #region Component Designer generated code

        /// <summary>
        /// Required method for Designer support - do not modify 
        /// the contents of this method with the code editor.
        /// </summary>
        private void InitializeComponent()
        {
            components = new System.ComponentModel.Container();
            _timer = new System.Windows.Forms.Timer(components);
            SuspendLayout();

            _timer.Enabled = true;
            _timer.Interval = 10;
            _timer.Tick += Timer_Tick;
            ResumeLayout(false);
        }

        #endregion

    }
}


Well, this is my first time to use Microsoft.Xna.Framework, and actually, I even tried the same thing by use of DirectX as the picture below.
But I don't know where the best coordinate point as the origin is for the drawing, I mean, different axes between Kinect's and Application's....
So, the knowledge of mathematics is needed..... about matrix or something like that.






Anyway, Kinect is very insteresting.

No comments:

Post a Comment