ADD : BodyStream Fonctionnel

KinectConnection/BodyImageStream.cs

@@ -7,80 +7,294 @@ using System.Threading.Tasks;
 using System.Windows;
 using System.Windows.Controls;
 using System.Windows.Media;
+using System.Windows.Media.Imaging;
 using System.Windows.Shapes;
 
 namespace KinectConnection
 {
     public class BodyImageStream : KinectStream
     {
-        private BodyFrameReader reader;
+        /// <summary>
-        private Body[] bodies = null;
+        /// The writeable bitmap.
-        private Canvas drawingCanvas = null;
+        /// </summary>
+        private WriteableBitmap bitmap = null;
 
-        public Body[] Bodies
+        // so that we can bind to it in the MainWindow.xaml
+        public WriteableBitmap Bitmap
         {
-            get { return bodies; }
+            get { return this.bitmap; }
-            set { SetProperty(ref bodies, value); }
         }
 
-        public Dictionary<JointType, Point> JointPoints { get; private set; }
+        private BodyFrameReader bodyFrameReader = null;
-
+        private Body[] bodies = null;
+        private DrawingGroup drawingGroup = new DrawingGroup();
+        private List<Pen> bodyColors = new List<Pen>();
+        private CoordinateMapper coordinateMapper = null;
+        private List<Tuple<JointType, JointType>> bones = new List<Tuple<JointType, JointType>>();
+        private readonly Brush trackedJointBrush = new SolidColorBrush(Color.FromArgb(255, 68, 192, 68));
+        private readonly Brush inferredJointBrush = Brushes.Yellow;
+        private const double HandSize = 30;
+        private const double JointThickness = 3;
+        private readonly Brush handClosedBrush = new SolidColorBrush(Color.FromArgb(128, 255, 0, 0));
+        private readonly Brush handOpenBrush = new SolidColorBrush(Color.FromArgb(128, 0, 255, 0));
+        private readonly Brush handLassoBrush = new SolidColorBrush(Color.FromArgb(128, 0, 0, 255));
+        private readonly Pen inferredBonePen = new Pen(Brushes.Gray, 1);
+        private const double ClipBoundsThickness = 10;
+        private DrawingImage imageSource = new DrawingImage();
+        private FrameDescription frameDescription;
+        private int displayHeight;
+        private int displayWidth;
 
+        public override ImageSource Source
+        {
+            get { return this.imageSource; }
+        }
 
         public BodyImageStream() : base()
         {
-            // Initialize the bodies array
+            this.coordinateMapper = this.KinectSensor.CoordinateMapper;
-            this.bodies = new Body[this.KinectSensor.BodyFrameSource.BodyCount];
+            frameDescription = this.KinectSensor.DepthFrameSource.FrameDescription;
-            JointPoints = new Dictionary<JointType, Point>();
+            displayHeight = frameDescription.Height;
+            displayWidth = frameDescription.Width;
+
+            // Torso
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.Head, JointType.Neck));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.Neck, JointType.SpineShoulder));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineShoulder, JointType.SpineMid));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineMid, JointType.SpineBase));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineShoulder, JointType.ShoulderRight));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineShoulder, JointType.ShoulderLeft));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineBase, JointType.HipRight));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.SpineBase, JointType.HipLeft));
+
+            // Right Arm
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.ShoulderRight, JointType.ElbowRight));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.ElbowRight, JointType.WristRight));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.WristRight, JointType.HandRight));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.HandRight, JointType.HandTipRight));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.WristRight, JointType.ThumbRight));
+
+            // Left Arm
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.ShoulderLeft, JointType.ElbowLeft));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.ElbowLeft, JointType.WristLeft));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.WristLeft, JointType.HandLeft));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.HandLeft, JointType.HandTipLeft));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.WristLeft, JointType.ThumbLeft));
+
+            // Right Leg
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.HipRight, JointType.KneeRight));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.KneeRight, JointType.AnkleRight));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.AnkleRight, JointType.FootRight));
+
+            // Left Leg
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.HipLeft, JointType.KneeLeft));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.KneeLeft, JointType.AnkleLeft));
+            this.bones.Add(new Tuple<JointType, JointType>(JointType.AnkleLeft, JointType.FootLeft));
+
+            this.bodyColors.Add(new Pen(Brushes.Red, 6));
+            this.bodyColors.Add(new Pen(Brushes.Orange, 6));
+            this.bodyColors.Add(new Pen(Brushes.Green, 6));
+            this.bodyColors.Add(new Pen(Brushes.Blue, 6));
+            this.bodyColors.Add(new Pen(Brushes.Indigo, 6));
+            this.bodyColors.Add(new Pen(Brushes.Violet, 6));
+
+            this.imageSource = new DrawingImage(this.drawingGroup);
         }
 
         public override void Start()
         {
-            // Open the reader for the body frames
+            this.bitmap = new WriteableBitmap(1920, 1080, 96.0, 96.0, PixelFormats.Pbgra32, null);
-            this.reader = this.KinectSensor.BodyFrameSource.OpenReader();
+
+            if (this.KinectSensor != null)
+            {
+                this.bodyFrameReader = this.KinectSensor.BodyFrameSource.OpenReader();
 
-            // Subscribe to the event
+                if (this.bodyFrameReader != null)
-            this.reader.FrameArrived += this.Reader_BodyFrameArrived;
+                {
+                    this.bodyFrameReader.FrameArrived += this.Reader_BodyFrameArrived;
+                }
+            }
         }
 
         public override void Stop()
         {
-            if (this.reader != null)
+            throw new NotImplementedException();
-            {
-                // Unsubscribe from the event
-                this.reader.FrameArrived -= this.Reader_BodyFrameArrived;
-
-                // Dispose the reader to free resources
-                this.reader.Dispose();
-                this.reader = null;
-            }
         }
 
         private void Reader_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
         {
+            bool dataReceived = false;
+
             using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
             {
                 if (bodyFrame != null)
                 {
+                    if (this.bodies == null)
+                    {
+                        this.bodies = new Body[bodyFrame.BodyCount];
+                    }
+
+                    // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
+                    // As long as those body objects are not disposed and not set to null in the array,
+                    // those body objects will be re-used.
                     bodyFrame.GetAndRefreshBodyData(this.bodies);
+                    dataReceived = true;
                 }
             }
 
-            // Process the body data
+            if (dataReceived)
-            foreach (var body in bodies)
+            {
+                using (DrawingContext dc = this.drawingGroup.Open())
+                {
+                    // Draw a transparent background to set the render size
+                    dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, displayWidth, displayHeight));
+
+                    int penIndex = 0;
+                    foreach (Body body in this.bodies)
                     {
+                        Pen drawPen = this.bodyColors[penIndex++];
+
                         if (body.IsTracked)
                         {
-                    foreach (JointType jointType in body.Joints.Keys)
+                            this.DrawClippedEdges(body, dc);
+
+                            IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
+
+                            // convert the joint points to depth (display) space
+                            Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
+
+                            foreach (JointType jointType in joints.Keys)
                             {
-                        // 3D 
+                                // sometimes the depth(Z) of an inferred joint may show as negative
-                        CameraSpacePoint cameraSpacePoint = body.Joints[jointType].Position;
+                                // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
-                        // 2D 
+                                CameraSpacePoint position = joints[jointType].Position;
-                        ColorSpacePoint colorSpacePoint = this.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(cameraSpacePoint);
+                                if (position.Z < 0)
+                                {
+                                    position.Z = 0.1f;
+                                }
 
-                        JointPoints[jointType] = new Point(colorSpacePoint.X, colorSpacePoint.Y);
+                                DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
+                                jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                             }
+
+                            this.DrawBody(joints, jointPoints, dc, drawPen);
+
+                            this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
+                            this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
+                        }
+                    }
+
+                    // prevent drawing outside of our render area
+                    this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, displayWidth, displayHeight));
+                }
+            }
+        }
+
+        private void DrawBody(IReadOnlyDictionary<JointType, Joint> joints, IDictionary<JointType, Point> jointPoints, DrawingContext drawingContext, Pen drawingPen)
+        {
+            // Draw the bones
+            foreach (var bone in this.bones)
+            {
+                this.DrawBone(joints, jointPoints, bone.Item1, bone.Item2, drawingContext, drawingPen);
             }
+
+            // Draw the joints
+            foreach (JointType jointType in joints.Keys)
+            {
+                Brush drawBrush = null;
+
+                TrackingState trackingState = joints[jointType].TrackingState;
+
+                if (trackingState == TrackingState.Tracked)
+                {
+                    drawBrush = this.trackedJointBrush;
+                }
+                else if (trackingState == TrackingState.Inferred)
+                {
+                    drawBrush = this.inferredJointBrush;
+                }
+
+                if (drawBrush != null)
+                {
+                    drawingContext.DrawEllipse(drawBrush, null, jointPoints[jointType], JointThickness, JointThickness);
+                }
+            }
+        }
+
+        private void DrawHand(HandState handState, Point handPosition, DrawingContext drawingContext)
+        {
+            switch (handState)
+            {
+                case HandState.Closed:
+                    drawingContext.DrawEllipse(this.handClosedBrush, null, handPosition, HandSize, HandSize);
+                    break;
+
+                case HandState.Open:
+                    drawingContext.DrawEllipse(this.handOpenBrush, null, handPosition, HandSize, HandSize);
+                    break;
+
+                case HandState.Lasso:
+                    drawingContext.DrawEllipse(this.handLassoBrush, null, handPosition, HandSize, HandSize);
+                    break;
+            }
+        }
+
+        private void DrawBone(IReadOnlyDictionary<JointType, Joint> joints, IDictionary<JointType, Point> jointPoints, JointType jointType0, JointType jointType1, DrawingContext drawingContext, Pen drawingPen)
+        {
+            Joint joint0 = joints[jointType0];
+            Joint joint1 = joints[jointType1];
+
+            // If we can't find either of these joints, exit
+            if (joint0.TrackingState == TrackingState.NotTracked ||
+            joint1.TrackingState == TrackingState.NotTracked)
+            {
+                return;
+            }
+
+            // We assume all drawn bones are inferred unless BOTH joints are tracked
+            Pen drawPen = this.inferredBonePen;
+            if ((joint0.TrackingState == TrackingState.Tracked) && (joint1.TrackingState == TrackingState.Tracked))
+            {
+                drawPen = drawingPen;
+            }
+
+            drawingContext.DrawLine(drawPen, jointPoints[jointType0], jointPoints[jointType1]);
+        }
+
+        private void DrawClippedEdges(Body body, DrawingContext drawingContext)
+        {
+            FrameEdges clippedEdges = body.ClippedEdges;
+
+            if (clippedEdges.HasFlag(FrameEdges.Bottom))
+            {
+                drawingContext.DrawRectangle(
+                    Brushes.Red,
+                    null,
+                    new Rect(0, displayHeight - ClipBoundsThickness, displayWidth, ClipBoundsThickness));
+            }
+
+            if (clippedEdges.HasFlag(FrameEdges.Top))
+            {
+                drawingContext.DrawRectangle(
+                    Brushes.Red,
+                    null,
+                    new Rect(0, 0, displayWidth, ClipBoundsThickness));
+            }
+
+            if (clippedEdges.HasFlag(FrameEdges.Left))
+            {
+                drawingContext.DrawRectangle(
+                    Brushes.Red,
+                    null,
+                    new Rect(0, 0, ClipBoundsThickness, displayHeight));
+            }
+
+            if (clippedEdges.HasFlag(FrameEdges.Right))
+            {
+                drawingContext.DrawRectangle(
+                    Brushes.Red,
+                    null,
+                    new Rect(displayWidth - ClipBoundsThickness, 0, ClipBoundsThickness, displayHeight));
             }
         }
     }

KinectConnection/ColorImageStream.cs

@@ -16,8 +16,7 @@ namespace KinectConnection
         /// </summary>
         private WriteableBitmap bitmap = null;
 
-        // so that we can bind to it in the MainWindow.xaml
+        public override ImageSource Source
-        public WriteableBitmap Bitmap
         {
             get { return this.bitmap; }
         }

KinectConnection/DepthImageStream.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Windows.Media;
 
 namespace KinectConnection
 {
@@ -7,6 +8,8 @@ namespace KinectConnection
     /// </summary>
     public class DepthImageStream : KinectStream
     {
+        public override ImageSource Source => throw new NotImplementedException();
+
         public override void Start()
         {
             throw new NotImplementedException();

KinectConnection/InfraredImageStream.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Windows.Media;
 
 namespace KinectConnection
 {
@@ -7,6 +8,8 @@ namespace KinectConnection
     /// </summary>
     public class InfraredImageStream : KinectStream
     {
+        public override ImageSource Source => throw new NotImplementedException();
+
         public override void Start()
         {
             throw new NotImplementedException();

KinectConnection/KinectStream.cs

@@ -1,5 +1,6 @@
 using CommunityToolkit.Mvvm.ComponentModel;
 using Microsoft.Kinect;
+using System.Windows.Media;
 
 namespace KinectConnection
 {
@@ -10,8 +11,11 @@ namespace KinectConnection
     {
         // Redondant d'avoir KinectSensor et KinectManager ici ? (car sensor dans manager)
         public KinectSensor KinectSensor { get; set; }
+        
         public KinectManager KinectManager { get; set; }
 
+        public abstract ImageSource Source { get; }
+
         public abstract void Start();
 
         public abstract void Stop();

KinectSensorStreams/KinectSensorStreams.csproj

@@ -12,6 +12,7 @@
 	<ItemGroup>
 		<PackageReference Include="CommunityToolkit.Mvvm" Version="8.2.2" />
 		<PackageReference Include="Microsoft.Extensions.DependencyInjection" Version="8.0.0" />
+		<PackageReference Include="Microsoft.Kinect" Version="2.0.1410.19000" />
 	</ItemGroup>
 
 	<ItemGroup>

KinectSensorStreams/View/MainWindow.xaml

@@ -114,8 +114,7 @@
                      HorizontalAlignment="Center"
                      Height="400"
                      Width="400">
-                <!--<Image Source="{Binding KinectStream.Bitmap}"/>-->
+                <Image Source="{Binding KinectStream.Source}"/>
-                <Grid x:Name="DisplayGrid" Background="Black" Width="512" Height="414"/>
             </Viewbox>
         </Grid>
     </Grid>

KinectSensorStreams/ViewModel/MainWindowVM.cs

@@ -58,7 +58,6 @@ namespace KinectSensorStreams.ViewModel
             // kinect stream => color stream for now
             KinectStream = KinectStreamsFactory[KinectStreams.Body];
 
-
             StartCommand = new RelayCommand(Start);
             // [Question] : StartCommand ici peut être mieux que BeginInit() dans MainWindow.xaml.cs ?
             ColorCommand = new RelayCommand(Color);