From f04be294ac416d75be413966fe7c85448f184412 Mon Sep 17 00:00:00 2001 From: lobroda Date: Fri, 26 Jan 2024 14:25:20 +0100 Subject: [PATCH] ADD : BodyStream Fonctionnel --- KinectConnection/BodyImageStream.cs | 280 +++++++++++++++--- KinectConnection/ColorImageStream.cs | 3 +- KinectConnection/DepthImageStream.cs | 3 + KinectConnection/InfraredImageStream.cs | 3 + KinectConnection/KinectStream.cs | 4 + .../KinectSensorStreams.csproj | 1 + KinectSensorStreams/View/MainWindow.xaml | 3 +- KinectSensorStreams/ViewModel/MainWindowVM.cs | 1 - 8 files changed, 260 insertions(+), 38 deletions(-) diff --git a/KinectConnection/BodyImageStream.cs b/KinectConnection/BodyImageStream.cs index 7cedc74..4f5a271 100644 --- a/KinectConnection/BodyImageStream.cs +++ b/KinectConnection/BodyImageStream.cs @@ -7,81 +7,295 @@ 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; - private Body[] bodies = null; - private Canvas drawingCanvas = null; + /// + /// The writeable bitmap. + /// + private WriteableBitmap bitmap = null; - public Body[] Bodies + // so that we can bind to it in the MainWindow.xaml + public WriteableBitmap Bitmap { - get { return bodies; } - set { SetProperty(ref bodies, value); } + get { return this.bitmap; } } - public Dictionary JointPoints { get; private set; } - + private BodyFrameReader bodyFrameReader = null; + private Body[] bodies = null; + private DrawingGroup drawingGroup = new DrawingGroup(); + private List bodyColors = new List(); + private CoordinateMapper coordinateMapper = null; + private List> bones = new List>(); + 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.bodies = new Body[this.KinectSensor.BodyFrameSource.BodyCount]; - JointPoints = new Dictionary(); + this.coordinateMapper = this.KinectSensor.CoordinateMapper; + frameDescription = this.KinectSensor.DepthFrameSource.FrameDescription; + displayHeight = frameDescription.Height; + displayWidth = frameDescription.Width; + + // Torso + this.bones.Add(new Tuple(JointType.Head, JointType.Neck)); + this.bones.Add(new Tuple(JointType.Neck, JointType.SpineShoulder)); + this.bones.Add(new Tuple(JointType.SpineShoulder, JointType.SpineMid)); + this.bones.Add(new Tuple(JointType.SpineMid, JointType.SpineBase)); + this.bones.Add(new Tuple(JointType.SpineShoulder, JointType.ShoulderRight)); + this.bones.Add(new Tuple(JointType.SpineShoulder, JointType.ShoulderLeft)); + this.bones.Add(new Tuple(JointType.SpineBase, JointType.HipRight)); + this.bones.Add(new Tuple(JointType.SpineBase, JointType.HipLeft)); + + // Right Arm + this.bones.Add(new Tuple(JointType.ShoulderRight, JointType.ElbowRight)); + this.bones.Add(new Tuple(JointType.ElbowRight, JointType.WristRight)); + this.bones.Add(new Tuple(JointType.WristRight, JointType.HandRight)); + this.bones.Add(new Tuple(JointType.HandRight, JointType.HandTipRight)); + this.bones.Add(new Tuple(JointType.WristRight, JointType.ThumbRight)); + + // Left Arm + this.bones.Add(new Tuple(JointType.ShoulderLeft, JointType.ElbowLeft)); + this.bones.Add(new Tuple(JointType.ElbowLeft, JointType.WristLeft)); + this.bones.Add(new Tuple(JointType.WristLeft, JointType.HandLeft)); + this.bones.Add(new Tuple(JointType.HandLeft, JointType.HandTipLeft)); + this.bones.Add(new Tuple(JointType.WristLeft, JointType.ThumbLeft)); + + // Right Leg + this.bones.Add(new Tuple(JointType.HipRight, JointType.KneeRight)); + this.bones.Add(new Tuple(JointType.KneeRight, JointType.AnkleRight)); + this.bones.Add(new Tuple(JointType.AnkleRight, JointType.FootRight)); + + // Left Leg + this.bones.Add(new Tuple(JointType.HipLeft, JointType.KneeLeft)); + this.bones.Add(new Tuple(JointType.KneeLeft, JointType.AnkleLeft)); + this.bones.Add(new Tuple(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.reader = this.KinectSensor.BodyFrameSource.OpenReader(); + this.bitmap = new WriteableBitmap(1920, 1080, 96.0, 96.0, PixelFormats.Pbgra32, null); + + if (this.KinectSensor != null) + { + this.bodyFrameReader = this.KinectSensor.BodyFrameSource.OpenReader(); - // Subscribe to the event - this.reader.FrameArrived += this.Reader_BodyFrameArrived; + if (this.bodyFrameReader != null) + { + this.bodyFrameReader.FrameArrived += this.Reader_BodyFrameArrived; + } + } } public override void Stop() { - if (this.reader != null) - { - // Unsubscribe from the event - this.reader.FrameArrived -= this.Reader_BodyFrameArrived; - - // Dispose the reader to free resources - this.reader.Dispose(); - this.reader = null; - } + throw new NotImplementedException(); } 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 - foreach (var body in bodies) + if (dataReceived) { - if (body.IsTracked) + using (DrawingContext dc = this.drawingGroup.Open()) { - foreach (JointType jointType in body.Joints.Keys) + // 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) { - // 3D - CameraSpacePoint cameraSpacePoint = body.Joints[jointType].Position; - // 2D - ColorSpacePoint colorSpacePoint = this.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(cameraSpacePoint); + Pen drawPen = this.bodyColors[penIndex++]; + + if (body.IsTracked) + { + this.DrawClippedEdges(body, dc); + + IReadOnlyDictionary joints = body.Joints; + + // convert the joint points to depth (display) space + Dictionary jointPoints = new Dictionary(); + + foreach (JointType jointType in joints.Keys) + { + // sometimes the depth(Z) of an inferred joint may show as negative + // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity) + CameraSpacePoint position = joints[jointType].Position; + if (position.Z < 0) + { + position.Z = 0.1f; + } + + DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); + jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); + } + + this.DrawBody(joints, jointPoints, dc, drawPen); - JointPoints[jointType] = new Point(colorSpacePoint.X, colorSpacePoint.Y); + 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 joints, IDictionary 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 joints, IDictionary 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)); + } + } } } diff --git a/KinectConnection/ColorImageStream.cs b/KinectConnection/ColorImageStream.cs index 794f393..bb43227 100644 --- a/KinectConnection/ColorImageStream.cs +++ b/KinectConnection/ColorImageStream.cs @@ -16,8 +16,7 @@ namespace KinectConnection /// private WriteableBitmap bitmap = null; - // so that we can bind to it in the MainWindow.xaml - public WriteableBitmap Bitmap + public override ImageSource Source { get { return this.bitmap; } } diff --git a/KinectConnection/DepthImageStream.cs b/KinectConnection/DepthImageStream.cs index 3d417f0..1e4d780 100644 --- a/KinectConnection/DepthImageStream.cs +++ b/KinectConnection/DepthImageStream.cs @@ -1,4 +1,5 @@ using System; +using System.Windows.Media; namespace KinectConnection { @@ -7,6 +8,8 @@ namespace KinectConnection /// public class DepthImageStream : KinectStream { + public override ImageSource Source => throw new NotImplementedException(); + public override void Start() { throw new NotImplementedException(); diff --git a/KinectConnection/InfraredImageStream.cs b/KinectConnection/InfraredImageStream.cs index d95fdb4..6bfed0c 100644 --- a/KinectConnection/InfraredImageStream.cs +++ b/KinectConnection/InfraredImageStream.cs @@ -1,4 +1,5 @@ using System; +using System.Windows.Media; namespace KinectConnection { @@ -7,6 +8,8 @@ namespace KinectConnection /// public class InfraredImageStream : KinectStream { + public override ImageSource Source => throw new NotImplementedException(); + public override void Start() { throw new NotImplementedException(); diff --git a/KinectConnection/KinectStream.cs b/KinectConnection/KinectStream.cs index a6debd0..c6890a2 100644 --- a/KinectConnection/KinectStream.cs +++ b/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(); diff --git a/KinectSensorStreams/KinectSensorStreams.csproj b/KinectSensorStreams/KinectSensorStreams.csproj index c63f408..37e7391 100644 --- a/KinectSensorStreams/KinectSensorStreams.csproj +++ b/KinectSensorStreams/KinectSensorStreams.csproj @@ -12,6 +12,7 @@ + diff --git a/KinectSensorStreams/View/MainWindow.xaml b/KinectSensorStreams/View/MainWindow.xaml index f33e8b6..3c6122d 100644 --- a/KinectSensorStreams/View/MainWindow.xaml +++ b/KinectSensorStreams/View/MainWindow.xaml @@ -114,8 +114,7 @@ HorizontalAlignment="Center" Height="400" Width="400"> - - + diff --git a/KinectSensorStreams/ViewModel/MainWindowVM.cs b/KinectSensorStreams/ViewModel/MainWindowVM.cs index f3daac6..024d111 100644 --- a/KinectSensorStreams/ViewModel/MainWindowVM.cs +++ b/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);