Display the angles of each joint of the skeleton in kinect c# - kinect

Question

How to display the angles of Kinect skeleton joints?

Answer 1

This is the programm you are looking for:
namespace Microsoft.Samples.Kinect.SkeletonBasics
{
using System.IO;
using System.IO.Ports;
using System.Threading;
using System;
using System.Windows;
using System.Windows.Media;
using Microsoft.Kinect;
using Microsoft.Kinect.Toolkit.Fusion;
using System.Windows.Media.Media3D;
public class Angles
{
public double AngleBetweenTwoVectors(Vector3D vectorA, Vector3D vectorB)
{
double dotProduct;
vectorA.Normalize();
vectorB.Normalize();
dotProduct = Vector3D.DotProduct(vectorA, vectorB);
return (double)Math.Acos(dotProduct)/Math.PI*180;
}
public byte[] GetVector(Skeleton skeleton)
{
Vector3D ShoulderCenter = new Vector3D(skeleton.Joints[JointType.ShoulderCenter].Position.X, skeleton.Joints[JointType.ShoulderCenter].Position.Y, skeleton.Joints[JointType.ShoulderCenter].Position.Z);
Vector3D RightShoulder = new Vector3D(skeleton.Joints[JointType.ShoulderRight].Position.X, skeleton.Joints[JointType.ShoulderRight].Position.Y, skeleton.Joints[JointType.ShoulderRight].Position.Z);
Vector3D LeftShoulder = new Vector3D(skeleton.Joints[JointType.ShoulderLeft].Position.X, skeleton.Joints[JointType.ShoulderLeft].Position.Y, skeleton.Joints[JointType.ShoulderLeft].Position.Z);
Vector3D RightElbow = new Vector3D(skeleton.Joints[JointType.ElbowRight].Position.X, skeleton.Joints[JointType.ElbowRight].Position.Y, skeleton.Joints[JointType.ElbowRight].Position.Z);
Vector3D LeftElbow = new Vector3D(skeleton.Joints[JointType.ElbowLeft].Position.X, skeleton.Joints[JointType.ElbowLeft].Position.Y, skeleton.Joints[JointType.ElbowLeft].Position.Z);
Vector3D RightWrist = new Vector3D(skeleton.Joints[JointType.WristRight].Position.X, skeleton.Joints[JointType.WristRight].Position.Y, skeleton.Joints[JointType.WristRight].Position.Z);
Vector3D LeftWrist = new Vector3D(skeleton.Joints[JointType.WristLeft].Position.X, skeleton.Joints[JointType.WristLeft].Position.Y, skeleton.Joints[JointType.WristLeft].Position.Z);
Vector3D UpVector = new Vector3D(0.0, 1.0, 0.0);
double AngleRightElbow = AngleBetweenTwoVectors(RightElbow - RightShoulder, RightElbow - RightWrist);
double AngleRightShoulder = AngleBetweenTwoVectors(UpVector, RightShoulder - RightElbow);
double AngleLeftElbow = AngleBetweenTwoVectors(LeftElbow - LeftShoulder, LeftElbow - LeftWrist);
double AngleLeftShoulder = AngleBetweenTwoVectors(UpVector, LeftShoulder - LeftElbow);
byte[] Angles = {Convert.ToByte(AngleRightElbow), Convert.ToByte(AngleRightShoulder),Convert.ToByte(AngleLeftElbow),Convert.ToByte(AngleLeftShoulder)};
return Angles;
}
}
/// Interaction logic for MainWindow.xaml
public partial class MainWindow : Window
{
SerialPort ArduinoPort;
/// Width of output drawing
private const float RenderWidth = 640.0f;
/// Height of our output drawing
private const float RenderHeight = 480.0f;
/// Thickness of drawn joint lines
private const double JointThickness = 3;
/// Thickness of body center ellipse
private const double BodyCenterThickness = 10;
/// Thickness of clip edge rectangles
private const double ClipBoundsThickness = 10;
/// Brush used to draw skeleton center point
private readonly Brush centerPointBrush = Brushes.Blue;
/// Brush used for drawing joints that are currently tracked
private readonly Brush trackedJointBrush = new SolidColorBrush(Color.FromArgb(255, 68, 192, 68));
/// Brush used for drawing joints that are currently inferred
private readonly Brush inferredJointBrush = Brushes.Yellow;
/// Pen used for drawing bones that are currently tracked
private readonly Pen trackedBonePen = new Pen(Brushes.Green, 6);
/// Pen used for drawing bones that are currently inferred
private readonly Pen inferredBonePen = new Pen(Brushes.Gray, 1);
/// Active Kinect sensor
private KinectSensor sensor;
/// Drawing group for skeleton rendering output
private DrawingGroup drawingGroup;
/// Drawing image that we will display
private DrawingImage imageSource;
/// Initializes a new instance of the MainWindow class.
public MainWindow()
{
InitializeComponent();
}
private static void RenderClippedEdges(Skeleton skeleton, DrawingContext drawingContext)
{
if (skeleton.ClippedEdges.HasFlag(FrameEdges.Bottom))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, RenderHeight - ClipBoundsThickness, RenderWidth, ClipBoundsThickness));
}
if (skeleton.ClippedEdges.HasFlag(FrameEdges.Top))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, RenderWidth, ClipBoundsThickness));
}
if (skeleton.ClippedEdges.HasFlag(FrameEdges.Left))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, ClipBoundsThickness, RenderHeight));
}
if (skeleton.ClippedEdges.HasFlag(FrameEdges.Right))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(RenderWidth - ClipBoundsThickness, 0, ClipBoundsThickness, RenderHeight));
}
}
private void WindowLoaded(object sender, RoutedEventArgs e)
{
// Create the drawing group we'll use for drawing
this.drawingGroup = new DrawingGroup();
// Create an image source that we can use in our image control
this.imageSource = new DrawingImage(this.drawingGroup);
// Display the drawing using our image control
Image.Source = this.imageSource;
// Look through all sensors and start the first connected one.
// This requires that a Kinect is connected at the time of app startup.
// To make your app robust against plug/unplug,
// it is recommended to use KinectSensorChooser provided in Microsoft.Kinect.Toolkit (See components in Toolkit Browser).
foreach (var potentialSensor in KinectSensor.KinectSensors)
{
if (potentialSensor.Status == KinectStatus.Connected)
{
this.sensor = potentialSensor;
break;
}
}
if (null != this.sensor)
{
// Turn on the skeleton stream to receive skeleton frames
this.sensor.SkeletonStream.Enable();
// Add an event handler to be called whenever there is new color frame data
this.sensor.SkeletonFrameReady += this.SensorSkeletonFrameReady;
// Start the sensor!
try
{
this.sensor.Start();
}
catch (IOException)
{
this.sensor = null;
}
}
if (null == this.sensor)
{
this.statusBarText.Text = Properties.Resources.NoKinectReady;
}
}
private void WindowClosing(object sender, System.ComponentModel.CancelEventArgs e)
{
if (null != this.sensor)
{
//ArduinoPort.Close();
this.sensor.Stop();
}
}
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
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, RenderWidth, RenderHeight));
if (skeletons.Length != 0)
{
foreach (Skeleton skel in skeletons)
{
RenderClippedEdges(skel, dc);
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
this.DrawBonesAndJoints(skel, dc);
Angles MyAngles = new Angles();
byte[] ReadyAngles = MyAngles.GetVector(skel);
RightElbow.Text = ReadyAngles[0].ToString();
RightShoulder.Text = ReadyAngles[1].ToString();
LeftElbow.Text = ReadyAngles[2].ToString();
LeftShoulder.Text = ReadyAngles[3].ToString();
byte[] SequenceStart = {255};
if (ArduinoPort.IsOpen)
{
ArduinoPort.Write(SequenceStart,0,1);
ArduinoPort.Write(ReadyAngles, 0, 4);
}
}
else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
{
dc.DrawEllipse(
this.centerPointBrush,
null,
this.SkeletonPointToScreen(skel.Position),
BodyCenterThickness,
BodyCenterThickness);
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
}
private void DrawBonesAndJoints(Skeleton skeleton, DrawingContext drawingContext)
{
// Render Torso
this.DrawBone(skeleton, drawingContext, JointType.Head, JointType.ShoulderCenter);
this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderLeft);
this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.ShoulderRight);
this.DrawBone(skeleton, drawingContext, JointType.ShoulderCenter, JointType.Spine);
this.DrawBone(skeleton, drawingContext, JointType.Spine, JointType.HipCenter);
this.DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipLeft);
this.DrawBone(skeleton, drawingContext, JointType.HipCenter, JointType.HipRight);
// Left Arm
this.DrawBone(skeleton, drawingContext, JointType.ShoulderLeft, JointType.ElbowLeft);
this.DrawBone(skeleton, drawingContext, JointType.ElbowLeft, JointType.WristLeft);
this.DrawBone(skeleton, drawingContext, JointType.WristLeft, JointType.HandLeft);
// Right Arm
this.DrawBone(skeleton, drawingContext, JointType.ShoulderRight, JointType.ElbowRight);
this.DrawBone(skeleton, drawingContext, JointType.ElbowRight, JointType.WristRight);
this.DrawBone(skeleton, drawingContext, JointType.WristRight, JointType.HandRight);
// Left Leg
this.DrawBone(skeleton, drawingContext, JointType.HipLeft, JointType.KneeLeft);
this.DrawBone(skeleton, drawingContext, JointType.KneeLeft, JointType.AnkleLeft);
this.DrawBone(skeleton, drawingContext, JointType.AnkleLeft, JointType.FootLeft);
// Right Leg
this.DrawBone(skeleton, drawingContext, JointType.HipRight, JointType.KneeRight);
this.DrawBone(skeleton, drawingContext, JointType.KneeRight, JointType.AnkleRight);
this.DrawBone(skeleton, drawingContext, JointType.AnkleRight, JointType.FootRight);
// Render Joints
foreach (Joint joint in skeleton.Joints)
{
Brush drawBrush = null;
if (joint.TrackingState == JointTrackingState.Tracked)
{
drawBrush = this.trackedJointBrush;
}
else if (joint.TrackingState == JointTrackingState.Inferred)
{
drawBrush = this.inferredJointBrush;
}
if (drawBrush != null)
{
drawingContext.DrawEllipse(drawBrush, null, this.SkeletonPointToScreen(joint.Position), JointThickness, JointThickness);
}
}
}
private Point SkeletonPointToScreen(SkeletonPoint skelpoint)
{
// Convert point to depth space.
// We are not using depth directly, but we do want the points in our 640x480 output resolution.
DepthImagePoint depthPoint = this.sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(skelpoint, DepthImageFormat.Resolution640x480Fps30);
return new Point(depthPoint.X, depthPoint.Y);
}
private void DrawBone(Skeleton skeleton, DrawingContext drawingContext, JointType jointType0, JointType jointType1)
{
Joint joint0 = skeleton.Joints[jointType0];
Joint joint1 = skeleton.Joints[jointType1];
// If we can't find either of these joints, exit
if (joint0.TrackingState == JointTrackingState.NotTracked ||
joint1.TrackingState == JointTrackingState.NotTracked)
{
return;
}
// Don't draw if both points are inferred
if (joint0.TrackingState == JointTrackingState.Inferred &&
joint1.TrackingState == JointTrackingState.Inferred)
{
return;
}
// We assume all drawn bones are inferred unless BOTH joints are tracked
Pen drawPen = this.inferredBonePen;
if (joint0.TrackingState == JointTrackingState.Tracked && joint1.TrackingState == JointTrackingState.Tracked)
{
drawPen = this.trackedBonePen;
}
drawingContext.DrawLine(drawPen, this.SkeletonPointToScreen(joint0.Position), this.SkeletonPointToScreen(joint1.Position));
}
private void CheckBoxSeatedModeChanged(object sender, RoutedEventArgs e)
{
if (null != this.sensor)
{
if (this.checkBoxSeatedMode.IsChecked.GetValueOrDefault())
{
this.sensor.SkeletonStream.TrackingMode = SkeletonTrackingMode.Seated;
}
else
{
this.sensor.SkeletonStream.TrackingMode = SkeletonTrackingMode.Default;
}
}
}
private void CheckBoxConnectArduino(object sender, RoutedEventArgs e)
{
if (this.checkArduinoConnected.IsChecked.GetValueOrDefault())
{
string MyCOM = "COM10";
ArduinoPort = new SerialPort(MyCOM, 9600);
ArduinoPort.Open();
}
else {
ArduinoPort.Close();
}
}
}
}
You also need to add the xaml:
<Window x:Class="Microsoft.Samples.Kinect.SkeletonBasics.MainWindow"
xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
Title="Skeleton Basics" Height="735" Width="770" Loaded="WindowLoaded" Closing="WindowClosing">
<Window.Resources>
<SolidColorBrush x:Key="MediumGreyBrush" Color="#ff6e6e6e"/>
<SolidColorBrush x:Key="KinectPurpleBrush" Color="#ff52318f"/>
<SolidColorBrush x:Key="KinectBlueBrush" Color="#ff00BCF2"/>
<Style TargetType="{x:Type Image}">
<Setter Property="SnapsToDevicePixels" Value="True"/>
</Style>
<Style TargetType="{x:Type CheckBox}" x:Key="SquareCheckBox" >
<Setter Property="Template">
<Setter.Value>
<ControlTemplate TargetType="{x:Type CheckBox}">
<Grid>
<StackPanel Orientation="Horizontal" Background="Transparent">
<Grid x:Name="SquareCheckBoxChecked">
<Image x:Name="CheckedNormal" Source="Images\CheckedNormal.png" Stretch="None" HorizontalAlignment="Center"/>
<Image x:Name="CheckedHover" Source="Images\CheckedHover.png" Stretch="None" HorizontalAlignment="Center" Visibility="Collapsed"/>
</Grid>
<Grid x:Name="SquareCheckBoxUnchecked" Visibility="Collapsed">
<Image x:Name="UncheckedNormal" Source="Images\UncheckedNormal.png" Stretch="None" HorizontalAlignment="Center"/>
<Image x:Name="UncheckedHover" Source="Images\UncheckedHover.png" Stretch="None" HorizontalAlignment="Center" Visibility="Collapsed"/>
</Grid>
<TextBlock x:Name="SquareCheckBoxText" Text="{TemplateBinding Content}" TextAlignment="Left" VerticalAlignment="Center" Foreground="{StaticResource KinectPurpleBrush}" FontSize="15" Margin="9,0,0,0"/>
</StackPanel>
</Grid>
<ControlTemplate.Triggers>
<Trigger Property="IsChecked" Value="false">
<Setter Property="Visibility" Value="Collapsed" TargetName="SquareCheckBoxChecked"/>
<Setter Property="Visibility" Value="Visible" TargetName="SquareCheckBoxUnchecked"/>
</Trigger>
<Trigger Property="IsMouseOver" Value="true">
<Setter Property="Visibility" Value="Collapsed" TargetName="CheckedNormal"/>
<Setter Property="Visibility" Value="Collapsed" TargetName="UncheckedNormal"/>
<Setter Property="Visibility" Value="Visible" TargetName="CheckedHover"/>
<Setter Property="Visibility" Value="Visible" TargetName="UncheckedHover"/>
<Setter Property="Foreground" Value="{StaticResource KinectBlueBrush}" TargetName="SquareCheckBoxText"/>
</Trigger>
</ControlTemplate.Triggers>
</ControlTemplate>
</Setter.Value>
</Setter>
</Style>
</Window.Resources>
<Grid Name="layoutGrid" Margin="10 0 10 0">
<Grid.RowDefinitions>
<RowDefinition Height="Auto"/>
<RowDefinition Height="*"/>
<RowDefinition Height="Auto"/>
<RowDefinition Height="Auto"/>
</Grid.RowDefinitions>
<DockPanel Grid.Row="0" Margin="0 0 0 20">
<Image DockPanel.Dock="Left" Source="Images\Logo.png" Stretch="Fill" Height="32" Width="81" Margin="0 10 0 5"/>
<TextBlock DockPanel.Dock="Right" Margin="0 0 -1 0" VerticalAlignment="Bottom" Foreground="{StaticResource MediumGreyBrush}" FontFamily="Segoe UI" FontSize="18">Skeleton Basics</TextBlock>
<Image Grid.Column="1" Source="Images\Status.png" Stretch="None" HorizontalAlignment="Center" Margin="0 0 0 5"/>
</DockPanel>
<Viewbox Grid.Row="1" Stretch="Uniform" HorizontalAlignment="Center">
<Image Name="Image" Width="640" Height="480"/>
</Viewbox>
<CheckBox Grid.Row="2" Style="{StaticResource SquareCheckBox}" Content="Connect Arduino" Height="Auto" HorizontalAlignment="Right" VerticalAlignment="Center" Margin="0 10 10 10" Name="checkArduinoConnected" Checked="CheckBoxConnectArduino" Unchecked="CheckBoxConnectArduino"/>
<CheckBox Grid.Row="2" Style="{StaticResource SquareCheckBox}" Content="Seated Mode" Height="Auto" HorizontalAlignment="Left" VerticalAlignment="Center" Margin="0 10 10 10" Name="checkBoxSeatedMode" Checked="CheckBoxSeatedModeChanged" Unchecked="CheckBoxSeatedModeChanged"/>
<StatusBar Grid.Row="3" HorizontalAlignment="Stretch" Name="statusBar" VerticalAlignment="Bottom" Background="White" Foreground="{StaticResource MediumGreyBrush}">
<StatusBarItem Padding="0 0 0 10">
<TextBlock Name="statusBarText" Margin="-1 0 0 0">Click 'Seated' to change skeletal pipeline type!</TextBlock>
</StatusBarItem>
</StatusBar>
<TextBox Name="RightShoulder" HorizontalAlignment="Left" Height="23" Margin="150,12,0,0" Grid.Row="2" TextWrapping="Wrap" VerticalAlignment="Top" Width="100"/>
<TextBox Name="RightElbow" HorizontalAlignment="Left" Height="23" Margin="250,12,0,0" Grid.Row="2" TextWrapping="Wrap" VerticalAlignment="Top" Width="100"/>
<TextBox Name="LeftShoulder" HorizontalAlignment="Left" Height="23" Margin="350,12,0,0" Grid.Row="2" TextWrapping="Wrap" VerticalAlignment="Top" Width="100"/>
<TextBox Name="LeftElbow" HorizontalAlignment="Left" Height="23" Margin="450,12,0,0" Grid.Row="2" TextWrapping="Wrap" VerticalAlignment="Top" Width="100"/>
</Grid>
</Window>
Of course you can not use the images, but that is not necessary. If this doesn't work right away just send me your e-mail and I will send you the complete source code :)

Answer 2

You can try Kinect Angles # http://drenton72.wordpress.com/2012/05/23/kinect-angles-v2-4/

Answer 3

I created Kinect Angles all I do is read the X,Y positions of the joints and then work out difference between the two positions as you would with vectors (just subtract one vector from the other one). Then I use simple trig to work out the angle of the vector. My Kinect Angles is open source so you should be able to figure it out from my source code.