body joints angle using kinect (checking time interval)

As you can see in image (the link is given below) when left hand is raised it shows an angle what i wanted is . A person should hold his arm for 5sec (in the position shown in a image) and if the person changes its arm position (that means if the angle is below 70 or above 80) within 5 sec some message should b displayed to put the arm back in same position and timer restarts!

http://postimage.org/image/hpfl41nzp/

Mainwindow.xaml file

    <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>

Main window.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);
    }


   }
   }

Draw joint class

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));





        }
      }

    }
  }

angle.cs class

 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));
   }

}

}

Would simply using a DispatchTimer for a countdown accomplish what you are wanting to do?

Psuedo-code follows:

private int isArmUpCounter = 5;
private bool isArmUp = false;

private void OnSkeletonReady() {
  if (myArm.Angle > 70 && myArm.Angle < 80)
  {
    if (isArmUp == false)
    {
      isArmUp = true;
      isArmUpCounter = 5;
      armUpTimer.Start();
    }
  }
  else
  {
    if (isArmUp == true && isArmUpCounter > 0)
    {
      Console.WriteLine("You dropped your arm too soon!");
    }

    isArmUp = false;
    armUpTimer.Stop();
  }
}

private void OnArmUpTimerTick() {
    // one a 1 second tick
    isArmUpCounter--;
}

A little tweaking of code certainly needed. But does the concept achieve what you're looking for?

来源：https://stackoverflow.com/questions/12608734/body-joints-angle-using-kinect-checking-time-interval

标签

kinect