kąt stawów ciała za pomocą kinect (sprawdzanie odstępu czasu)
Jak widać na zdjęciu (link jest podany poniżej), gdy lewa ręka jest podniesiona, pokazuje kąt, który chciałem. Osoba powinna trzymać rękę przez 5 sekund (w pozycji pokazanej na obrazku), a jeśli osoba zmieni pozycję ramienia (to znaczy, jeśli kąt jest poniżej 70 lub powyżej 80) w ciągu 5 sekund, w celu umieszczenia ramienia powinien zostać wyświetlony komunikat w tej samej pozycji i ponownie uruchamia się timer!
http://postimage.org/image/hpfl41nzp/
Plik Mainwindow.xaml
<Window x:Class="shoulder_joint.MainWindow"
xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
Title="MainWindow" Height="511" Width="525" Loaded="Window_Loaded"
Closing="Window_Closing" >
<Grid Height="479" Width="510">
<Grid.ColumnDefinitions>
<ColumnDefinition Width="Auto" />
<ColumnDefinition Width="6*" />
<ColumnDefinition Width="497*" />
</Grid.ColumnDefinitions>
<Image Height="479" HorizontalAlignment="Left" Name="frame_image" Stretch="Fill"
VerticalAlignment="Top" Width="503" Grid.Column="1" Grid.ColumnSpan="2" />
<Image Height="131" Margin="14,14,319,0" Name="Color_image" Stretch="Fill"
VerticalAlignment="Top" Grid.Column="2" />
</Grid>
</Window>
Główne okno.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 Microsoft.Xna.Framework;
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 Microsoft.Kinect;
namespace shoulder_joint
{
/// <summary>
/// Interaction logic for MainWindow.xaml
/// </summary>
public partial class MainWindow : Window
{
bool flag = false;
angle angle1 = new angle();
Draw_joint draw = new Draw_joint();
int joint_thick =10;
private readonly Brush detect_brush = Brushes.GreenYellow;
private readonly Brush inferred_brush = Brushes.Indigo;
private readonly Pen detect_pen = new Pen( Brushes.Black,6);
private readonly Pen inferred_pen = new Pen (Brushes.DarkOliveGreen,2);
KinectSensor sensor;
Skeleton[] skelton;
int width =491;
int height =470;
DrawingGroup drawing_group;
DrawingImage draw_image;
byte[] color_pixel;
WriteableBitmap bitmap;
public MainWindow()
{
InitializeComponent();
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
foreach (var potential_sensor in KinectSensor.KinectSensors)
{
if (potential_sensor.Status == KinectStatus.Connected)
{
sensor = potential_sensor;
break;
}
}
sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
sensor.SkeletonStream.Enable();
color_pixel = new byte [sensor.ColorStream.FramePixelDataLength];
bitmap = new WriteableBitmap(sensor.ColorStream.FrameWidth,
sensor.ColorStream.FrameHeight,96,96,PixelFormats.Bgr32,null);
Color_image.Source = bitmap;
drawing_group = new DrawingGroup();
draw_image = new DrawingImage(drawing_group);
frame_image.Source = draw_image;
sensor.SkeletonFrameReady += SensorSkeltonFrameReady;
sensor.ColorFrameReady += SensorColorFrameReady;
sensor.Start();
}
private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
{
if (sensor != null)
{
sensor.Stop();
}
}
private void SensorColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
{
ColorImageFrame color_frame;
using (color_frame = e.OpenColorImageFrame())
{
if (color_frame != null)
{
color_frame.CopyPixelDataTo(color_pixel);
bitmap.WritePixels(new Int32Rect(0,
0,bitmap.PixelWidth,bitmap.PixelHeight),
this.color_pixel,
this.bitmap.PixelWidth* sizeof(int),
0);
}
}
}
private void SensorSkeltonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
SkeletonFrame skelton_frame;
using (skelton_frame = e.OpenSkeletonFrame())
{
if (skelton_frame != null)
{
skelton = new Skeleton[skelton_frame.SkeletonArrayLength];
skelton_frame.CopySkeletonDataTo(skelton);
using (DrawingContext dc = drawing_group.Open())
{
dc.DrawRectangle(Brushes.Gray, null, new Rect(0, 0, width,
height));
if (skelton != null)
{
foreach (Skeleton skel in skelton)
{
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
this.find_pos_joints(skel, dc);
// this.angle_between(dc);
}
}
}
drawing_group.ClipGeometry = new RectangleGeometry(new
Rect(0,0,width,height));
}
}
}
}
private void find_pos_joints(Skeleton sk, DrawingContext draw_contex)
{
draw.Draw_joint1(sk, draw_contex, JointType.ShoulderCenter,
JointType.ShoulderRight,
JointType.ElbowRight,JointType.WristRight,
JointType.ShoulderLeft,JointType.ElbowLeft,JointType.WristLeft,
JointType.Head,JointType.HipCenter,JointType.HipLeft,
JointType.KneeLeft,JointType.AnkleLeft,
detect_brush,inferred_brush,joint_thick,inferred_pen,detect_pen,sensor);
angle1.angle_between_right_shoulder(sk,
draw_contex,JointType.Head,JointType.Spine ,JointType.ShoulderCenter,
JointType.ShoulderRight,JointType.ElbowRight,sensor);
angle1.angle_between_left_shoulder(sk, draw_contex, JointType.ShoulderCenter,
JointType.ShoulderLeft, JointType.ElbowLeft, sensor);
angle1.angle_between_left_leg(sk,
draw_contex,JointType.HipLeft,JointType.KneeLeft,JointType.AnkleLeft,sensor);
}
}
}
Narysuj klasę stawów
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using System.Windows.Media;
namespace shoulder_joint
{
class Draw_joint
{
angle angle1 = new angle();
public void Draw_joint1(Skeleton sk1, DrawingContext dc1, JointType joint1,
JointType joint2,
JointType joint3, JointType joint4, JointType joint5, JointType joint6,
JointType joint7,
JointType joint8 ,JointType joint9,JointType joint100,JointType joint101,
JointType joint102
,Brush det_brush,
Brush infer_brush,int thick, Pen det_pen,Pen infer_pen,KinectSensor sensor)
{
Joint joint11 = sk1.Joints[joint1];
Joint joint22 = sk1.Joints[joint2];
Joint joint33 = sk1.Joints[joint3];
Joint joint44 = sk1.Joints[joint4];
Joint joint55 = sk1.Joints[joint5];
Joint joint66 = sk1.Joints[joint6];
Joint joint77 = sk1.Joints[joint7];
Joint joint88 = sk1.Joints[joint8];
Joint joint99 = sk1.Joints[joint9];
Joint joint10 = sk1.Joints[joint100];
Joint joint1011 = sk1.Joints[joint101];
Joint joint1022 = sk1.Joints[joint102];
if (joint11.TrackingState == JointTrackingState.Tracked &&
joint22.TrackingState == JointTrackingState.Tracked &&
joint33.TrackingState == JointTrackingState.Tracked &&
joint44.TrackingState == JointTrackingState.Tracked &&
joint55.TrackingState == JointTrackingState.Tracked &&
joint66.TrackingState == JointTrackingState.Tracked &&
joint77.TrackingState == JointTrackingState.Tracked &&
joint88.TrackingState == JointTrackingState.Tracked &&
joint99.TrackingState == JointTrackingState.Tracked &&
joint10.TrackingState == JointTrackingState.Tracked &&
joint1011.TrackingState == JointTrackingState.Tracked &&
joint1022.TrackingState == JointTrackingState.Tracked
)
{
dc1.DrawEllipse(det_brush, det_pen, angle1.point_toScreen(joint11.Position,
sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen, angle1.point_toScreen(joint22.Position,
sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen, angle1.point_toScreen(joint33.Position,
sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen, angle1.point_toScreen(joint44.Position,
sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen, angle1.point_toScreen(joint55.Position,
sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen, angle1.point_toScreen(joint66.Position,
sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen, angle1.point_toScreen(joint77.Position,
sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen, angle1.point_toScreen(joint88.Position,
sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen, angle1.point_toScreen(joint99.Position,
sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen, angle1.point_toScreen(joint10.Position,
sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen,
angle1.point_toScreen(joint1011.Position, sensor), thick, thick);
dc1.DrawEllipse(det_brush, det_pen,
angle1.point_toScreen(joint1022.Position, sensor), thick, thick);
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint11.Position, sensor),
angle1.point_toScreen(joint22.Position, sensor));
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint11.Position, sensor),
angle1.point_toScreen(joint88.Position, sensor));
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint11.Position, sensor),
angle1.point_toScreen(joint99.Position, sensor));
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint22.Position, sensor),
angle1.point_toScreen(joint33.Position, sensor));
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint33.Position, sensor),
angle1.point_toScreen(joint44.Position, sensor));
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint11.Position, sensor),
angle1.point_toScreen(joint55.Position, sensor));
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint55.Position, sensor),
angle1.point_toScreen(joint66.Position, sensor));
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint66.Position, sensor),
angle1.point_toScreen(joint77.Position, sensor));
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint99.Position, sensor),
angle1.point_toScreen(joint10.Position, sensor));
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint10.Position, sensor),
angle1.point_toScreen(joint1011.Position, sensor));
dc1.DrawLine(infer_pen, angle1.point_toScreen(joint1011.Position, sensor),
angle1.point_toScreen(joint1022.Position, sensor));
}
}
}
}
klasa angle.cs
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using System.Windows.Media;
using Microsoft.Xna.Framework;
using System.Windows;
namespace shoulder_joint
{
class angle
{
public void angle_between_left_shoulder(Skeleton sk1, DrawingContext draw,
JointType Shoulder_cntre,
JointType Shoulder_left, JointType Elbow_left, KinectSensor sen)
{
Joint sh_cntr = sk1.Joints[Shoulder_cntre];
Joint sh_left = sk1.Joints[Shoulder_left];
Joint elb_left = sk1.Joints[Elbow_left];
Vector3 v_shoulder = new Vector3(sh_cntr.Position.X, sh_cntr.Position.Y,
sh_cntr.Position.Z);
Vector3 v_should_l = new Vector3(sh_left.Position.X, sh_left.Position.Y,
sh_left.Position.Z);
Vector3 v_elbow_l = new Vector3(elb_left.Position.X, elb_left.Position.Y,
elb_left.Position.Z);
Vector3 va = v_shoulder - v_should_l;
Vector3 vb = v_elbow_l - v_should_l;
va = Vector3.Normalize(va);
vb = Vector3.Normalize(vb);
float len_prod = va.Length() * va.Length();
float dot_pro = Vector3.Dot(va, vb);
double angle = Math.Acos(dot_pro);
angle = angle * 180 / Math.PI;
angle = 180 - angle;
System.Windows.Point shoul_l = this.point_toScreen(sh_left.Position, sen);
draw.DrawText(new FormattedText(angle.ToString("0"), new
System.Globalization.CultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"), 16, Brushes.WhiteSmoke),
new System.Windows.Point(shoul_l.X+10, shoul_l.Y +20));
}
public void angle_between_right_shoulder(Skeleton sk1, DrawingContext draw,
JointType head, JointType spine,
JointType Shoulder_cntre, JointType Shoulder_right, JointType
Elbow_right,KinectSensor sen)
{
Joint sh_cntr = sk1.Joints[Shoulder_cntre];
Joint sh_right = sk1.Joints[Shoulder_right];
Joint elb_right = sk1.Joints[Elbow_right];
Vector3 v_shoulder = new Vector3(sh_cntr.Position.X, sh_cntr.Position.Y,
sh_cntr.Position.Z);
Vector3 v_should_r = new Vector3(sh_right.Position.X, sh_right.Position.Y,
sh_right.Position.Z);
Vector3 v_elbow_r = new Vector3(elb_right.Position.X, elb_right.Position.Y,
elb_right.Position.Z);
Vector3 newv1 =v_shoulder - v_should_r ;
Vector3 newv2 = v_elbow_r - v_should_r;
newv1 = Vector3.Normalize(newv1);
newv2 = Vector3.Normalize(newv2);
float len_prod = newv1.Length() * newv2.Length();
float dot_pro = Vector3.Dot(newv1, newv2);
double angle = Math.Acos(dot_pro);
angle = angle * 180 / Math.PI;
angle = 180 - angle;
System.Windows.Point shoul_r = this.point_toScreen(sh_right.Position, sen);
draw.DrawText(new FormattedText(angle.ToString("0"), new
System.Globalization.CultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"), 16, Brushes.WhiteSmoke),
new System.Windows.Point(shoul_r.X-15, shoul_r.Y+20));
}
public System.Windows.Point point_toScreen(SkeletonPoint point,KinectSensor sensor)
{
DepthImagePoint depth_point = sensor.MapSkeletonPointToDepth(point,
DepthImageFormat.Resolution640x480Fps30);
return new System.Windows.Point(depth_point.X, depth_point.Y);
}
public void angle_between_left_leg(Skeleton sk, DrawingContext draw_contex,
JointType hip_l, JointType knee_l,
JointType ankle_l,KinectSensor sen)
{
Joint Hl = sk.Joints[hip_l];
Joint kl = sk.Joints[knee_l];
Joint Al = sk.Joints[ankle_l];
Vector3 hip_left = new Vector3(Hl.Position.X,Hl.Position.Y,Hl.Position.Z);
Vector3 knee_left = new Vector3(kl.Position.X, kl.Position.Y, kl.Position.Z);
Vector3 ankle_left = new Vector3(Al.Position.X, Al.Position.Y, Al.Position.Z);
Vector3 va = hip_left - knee_left;
Vector3 vb = ankle_left - knee_left;
float len_prod = va.Length() * vb.Length();
float dot_pro = Vector3.Dot(va, vb);
double angle = Math.Acos(dot_pro/len_prod);
angle = angle * 180 / Math.PI;
angle = 180 - angle;
System.Windows.Point shoul_r = this.point_toScreen(kl.Position, sen);
draw_contex.DrawText(new FormattedText(angle.ToString("0"), new
System.Globalization.CultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"), 16, Brushes.WhiteSmoke),
new System.Windows.Point(shoul_r.X - 15, shoul_r.Y + 20));
}
}
}