kinect2与c#之彩色图像与深度图像叠加（静态姿势识别）参考代码_.NET_编程开发_程序员俱乐部

因为自己之前在摸索kinect，下面是参考官网已经别人写的一些代码，自己也模仿写了一个深度图像和彩色图像叠加的代码，代码里面有自己在做简单的静态手势时候的一些代码，部分我自己注释掉了，大家注意如果不需要的话可以直接省略的。

注意的是，我用的是kinect v2和vs2013执行环境，脚本语言使用的是c#+WPF。

注：直接就上代码，以后有时间把kinect的安装配置再详细写个文档吧！！

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
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 System.IO;
using Microsoft.Kinect;//添加对Kinect对象的引用

namespace WpfApplication1
{
/// <summary>
/// MainWindow.xaml 的交互逻辑
/// </summary>
public partial class MainWindow : Window
{
//定义相关变量
#region
private KinectSensor kinectSensor = null;
private MultiSourceFrameReader multiFrameSourceReader = null;//多源阅读器
private WriteableBitmap Bitmap = null;
private WriteableBitmap colorBitmap = null;

private CoordinateMapper coordinateMapper = null;//坐标映射器

private const float InferredZPositionClamp = 0.1f;
//private readonly Brush trackedJointBrush = new SolidColorBrush(Color.FromArgb(255, 68, 192, 68));
private readonly Brush trackedJointBrush = Brushes.Red;
private readonly Brush inferredJointBrush = Brushes.Blue;

private DrawingGroup drawingGroup;//DrawingGroup表示可以作为单个绘图进行运算的绘图集合
private DrawingImage imageSource;
private Body[] bodies = null;
private int displayWidth;
private int displayHeight;
private List<Tuple<JointType, JointType>> bones;
private List<Pen> bodyColors;
public bool Savejudge;

private Pose[] poseLibrary;//自定义手势库对象
ImageBrush imgBrush = new ImageBrush();
#endregion
public MainWindow()
{
this.kinectSensor = KinectSensor.GetDefault();
this.multiFrameSourceReader = this.kinectSensor.OpenMultiSourceFrameReader(FrameSourceTypes.Color | FrameSourceTypes.Body | FrameSourceTypes.Depth);
this.multiFrameSourceReader.MultiSourceFrameArrived += this.Reader_MultiSourceFrameArrived;
this.coordinateMapper = this.kinectSensor.CoordinateMapper;
FrameDescription colorFrameDescription = this.kinectSensor.ColorFrameSource.FrameDescription;
this.Bitmap = new WriteableBitmap(colorFrameDescription.Width, colorFrameDescription.Height, 96.0, 96.0, PixelFormats.Bgr32, null);
this.colorBitmap = new WriteableBitmap(colorFrameDescription.Width, colorFrameDescription.Height, 96.0, 96.0, PixelFormats.Bgr32, null);

//color
this.displayWidth = colorFrameDescription.Width;
this.displayHeight = colorFrameDescription.Height;
//关节之间的线定义为Bone
this.bones = new List<Tuple<JointType, JointType>>();
//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));

//body
//每个BodyIndex一个填充颜色,
this.bodyColors = new List<Pen>();
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.kinectSensor.Open();
this.drawingGroup = new DrawingGroup();
this.imageSource = new DrawingImage(this.drawingGroup);
this.Bitmap = new WriteableBitmap(colorFrameDescription.Width, colorFrameDescription.Height, 96.0, 96.0, PixelFormats.Bgr32, null);
this.DataContext = this;
this.Savejudge = false;
InitializeComponent();
PopulatePoseLibrary();

}

public struct Pose//名为Pose的结构
{//Pose存储了一个姿势的名称和一个PoseAngle数组
public string Title;
public PoseAngle[] Angles;
}

public class PoseAngle//创建一个新的PoseAngle类
{
public PoseAngle(JointType startJoint, JointType angleJoint, JointType endJoint, double angle, double threshold)
{//有两个JointType类型的成员变量用来计算角度，Angle为期望角度，Threshold 阈值
StartJoint = startJoint;
AngleJoint = angleJoint;
EndJoint = endJoint;
Angle = angle;
Threshold = threshold;
}
public JointType StartJoint { get; private set; }
public JointType AngleJoint { get; private set; }
public JointType EndJoint { get; private set; }
public double Angle { get; private set; }
public double Threshold { get; private set; }
}

private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
DepthFrame depthFrame = null;
ColorFrame colorFrame = null;
BodyFrame bodyFrame = null;
//bool isBitmapLocked = false;

MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
if (multiSourceFrame == null)
{
return;
}
try
{
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame();
//Process Body
bool dataReceived = false;
if ((depthFrame == null) || (colorFrame == null) || (bodyFrame == null))
{
return;
}
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];//BodyFrame中的人体总数
}
bodyFrame.GetAndRefreshBodyData(this.bodies);//数据带入
dataReceived = true;

if (this.Savejudge)
{
SaveAllJoints(this.bodies);
this.Savejudge = false;
}

if (dataReceived)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
// 画一个透明背景设置渲染的大小（绘制矩形，透明背景）
dc.DrawRectangle(new SolidColorBrush(Color.FromArgb(0, 0, 0, 0)), null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

int penIndex = 0;
foreach (Body body in this.bodies)
{
Pen drawPen = this.bodyColors[penIndex++];

if (body.IsTracked)
{
IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
CameraSpacePoint position = joints[jointType].Position;
//这个防止是照抄的相机转深度，不知是否奏效或应该
if (position.Z < 0)
{
position.Z = 0.1f;
}
//相机点映射到彩色空间点
ColorSpacePoint colorSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position);
jointPoints[jointType] = new Point(colorSpacePoint.X, colorSpacePoint.Y);//映射完的坐标替换旧坐标

}
this.DrawBody(joints, jointPoints, dc, drawPen);
double angle1 = GetJointAngle1(body.Joints[JointType.ShoulderRight], body.Joints[JointType.ElbowRight], body.Joints[JointType.WristRight]);

this.ProcessPosePerforming(body);
}
}
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}

}

// Process Color
FrameDescription colorFrameDescription = colorFrame.FrameDescription;
using (KinectBuffer colorBuffer = colorFrame.LockRawImageBuffer())
{
this.colorBitmap.Lock();
if ((colorFrameDescription.Width == this.colorBitmap.PixelWidth) && (colorFrameDescription.Height == this.colorBitmap.Height))
{
colorFrame.CopyConvertedFrameDataToIntPtr(
this.colorBitmap.BackBuffer,
(uint)(colorFrameDescription.Width * colorFrameDescription.Height * 4),
ColorImageFormat.Bgra);//数据拷至位图
this.colorBitmap.AddDirtyRect(new Int32Rect(0, 0, this.colorBitmap.PixelWidth, this.colorBitmap.PixelHeight));//更改制定位图数据

}
this.colorBitmap.Unlock();
}
}
finally
{
//if (isBitmapLocked) this.bitmap.Unlock();
if (depthFrame != null) depthFrame.Dispose();
if (colorFrame != null) colorFrame.Dispose();
if (bodyFrame != null) bodyFrame.Dispose();
}
}

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],15.0, 15.0);
}
}
}

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 (joint0.TrackingState == TrackingState.NotTracked ||
joint1.TrackingState == TrackingState.NotTracked)
{
return;
}

// 除非关节已跟踪，我们假定所有骨头都推断，画笔为推断默认色
Pen drawPen = new Pen(Brushes.Yellow, 18);
if ((joint0.TrackingState == TrackingState.Tracked) && (joint1.TrackingState == TrackingState.Tracked))
{
drawPen = drawingPen;//笔换色
}

drawingContext.DrawLine(drawPen, jointPoints[jointType0], jointPoints[jointType1]);
}

//计算三个关节所成角度的方法
private double GetJointAngle1(Joint startJoint, Joint angleJoint, Joint endJoint)
{
Console.WriteLine("开始计算角度");
Point startPoint = GetJointPointScreen(startJoint);//通过GetJointPoint方法将坐标转换到UI界面上计算角度，方便我们进行测试
Point anglePoint = GetJointPointScreen(angleJoint);
Point endPoint = GetJointPointScreen(endJoint);

//double a;
//double b;
//double c;
double aa;
double bb;
double cc;
//三维空间计算关节角度
aa = Math.Sqrt(Math.Pow(startJoint.Position.X - angleJoint.Position.X, 2) + Math.Pow(startJoint.Position.Y - angleJoint.Position.Y, 2)+Math.Pow(startJoint.Position.Z- angleJoint.Position.Z, 2));
Console.WriteLine("aa={0}", aa);
bb = Math.Sqrt(Math.Pow(endJoint.Position.X - angleJoint.Position.X, 2) + Math.Pow(endJoint.Position.Y - angleJoint.Position.Y, 2) + Math.Pow(endJoint.Position.Z - angleJoint.Position.Z, 2));
Console.WriteLine("bb={0}", bb);
cc = Math.Sqrt(Math.Pow(startJoint.Position.X - endJoint.Position.X, 2) + Math.Pow(startJoint.Position.Y - endJoint.Position.Y, 2) + Math.Pow(startJoint.Position.Z - endJoint.Position.Z, 2));
Console.WriteLine("cc={0}", cc);
double angleRadtest = Math.Acos((aa * aa + bb * bb - cc * cc) / (2 * aa * bb));//它的返回结果是弧度，下一步需要转换为度数
double angleDegtest = angleRadtest * 180 / Math.PI;
Console.WriteLine("angleDegtest={0}", angleDegtest);
return angleDegtest;

//UI平面计算关节角度
//a = Math.Sqrt(Math.Pow(startPoint.X - anglePoint.X, 2) + Math.Pow(startPoint.Y - anglePoint.Y, 2));
//Console.WriteLine("a={0}", a);
//b = Math.Sqrt(Math.Pow(anglePoint.X - endPoint.X, 2) + Math.Pow(anglePoint.Y - endPoint.Y, 2));
//Console.WriteLine("b={0}", b);
//c = Math.Sqrt(Math.Pow(startPoint.X - endPoint.X, 2) + Math.Pow(startPoint.Y - endPoint.Y, 2));
//Console.WriteLine("c={0}", c);
//double angleRad = Math.Acos((a * a + b * b - c * c) / (2 * a * b));//它的返回结果是弧度，下一步需要转换为度数
//double angleDeg = angleRad * 180 / Math.PI;

//if (primaryPoint.Y < anglePoint.Y)//If语句处理角度值在180-360的情况
//{
// angleDeg = 360 - angleDeg;//余弦定理返回的角度在0-180度内，if语句将在第三和第四象限的值调整到第一第二象限中来。
//}
//Console.WriteLine("开始输出角度值");
//Console.WriteLine("角度是：{0}", angleRad);
//return angleDeg;
}

//计算三个关节角度面与XOZ面所成角度
private double GetJointAngle2(Joint startJoint, Joint angleJoint, Joint endJoint)
{
double x1 = startJoint.Position.X - angleJoint.Position.X;
double y1 = startJoint.Position.Y - angleJoint.Position.Y;
double z1 = startJoint.Position.Z - angleJoint.Position.Z;
double x2 = endJoint.Position.X - angleJoint.Position.X;
double y2 = endJoint.Position.Y - angleJoint.Position.Y;
double z2 = endJoint.Position.Z - angleJoint.Position.Z;
//肩膀肘关节和手腕所成面的法向量
double nx1 = (y1 * z2 - z1 * y2) * 1000;
double ny1 = (z1 * x2 - x1 * z2) * 1000;
double nz1 = (x1 * y2 - x2 * y1) * 1000;
Console.WriteLine("nx1={0}", nx1);
Console.WriteLine("ny1={0}", ny1);
Console.WriteLine("nz1={0}", nz1);
//取一个XOZ平面所成的法向量
double nx2 = 0;
double ny2 = -100;
double nz2 = 0;
double twoPlaneAngle = Math.Acos((nx1 * nx2 + ny1 * ny2 + nz1 * nz2) / ((Math.Sqrt(nx1 * nx1 + ny1 * ny1 + nz1 * nz1)) * (Math.Sqrt(nx2 * nx2 + ny2 * ny2 + nz2 * nz2))));
Console.WriteLine("twoPlaneAngle={0}", twoPlaneAngle);
double PlaneAngle = twoPlaneAngle * 180 / Math.PI;
Console.WriteLine("肩膀肘关节和手腕所成面与XOZ所成角为:{0}", PlaneAngle);
return PlaneAngle;
}

//自定义姿势库
private void PopulatePoseLibrary()
{
this.poseLibrary = new Pose[7];

//第一个姿势
this.poseLibrary[0] = new Pose();
this.poseLibrary[0].Title = "举起双手";
this.poseLibrary[0].Angles = new PoseAngle[4];
this.poseLibrary[0].Angles[0] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft,JointType.WristLeft, 90, 10);
this.poseLibrary[0].Angles[1] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 90, 10);
this.poseLibrary[0].Angles[2] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 90, 10);
this.poseLibrary[0].Angles[3] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 90, 10);

//第二个手势
this.poseLibrary[1] = new Pose();
this.poseLibrary[1].Title = "双手放在胸前";
this.poseLibrary[1].Angles = new PoseAngle[4];
this.poseLibrary[1].Angles[0] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft,45,10);
this.poseLibrary[1].Angles[1] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 180, 10);
this.poseLibrary[1].Angles[2] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 45, 10);
this.poseLibrary[1].Angles[3] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight,180, 10);

//第三个手势
this.poseLibrary[2] = new Pose();
this.poseLibrary[2].Title = "双手伸直放在前面";
this.poseLibrary[2].Angles = new PoseAngle[4];
this.poseLibrary[2].Angles[0] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 180, 10);
this.poseLibrary[2].Angles[1] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 0, 10);
this.poseLibrary[2].Angles[2] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 180, 10);
this.poseLibrary[2].Angles[3] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 0, 10);

//第四个手势
this.poseLibrary[3] = new Pose();
this.poseLibrary[3].Title = "右手举起";
this.poseLibrary[3].Angles = new PoseAngle[4];
this.poseLibrary[3].Angles[0] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 180, 10);
this.poseLibrary[3].Angles[1] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 180, 10);
this.poseLibrary[3].Angles[2] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 90, 10);
this.poseLibrary[3].Angles[3] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 90, 10);

//第五个手势
this.poseLibrary[4] = new Pose();
this.poseLibrary[4].Title = "右手抬起，左手与身成角";
this.poseLibrary[4].Angles = new PoseAngle[4];
this.poseLibrary[4].Angles[0] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 180, 10);
this.poseLibrary[4].Angles[1] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 135, 10);
this.poseLibrary[4].Angles[2] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 90, 10);
this.poseLibrary[4].Angles[3] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 90, 10);

//第六个手势
this.poseLibrary[5] = new Pose();
this.poseLibrary[5].Title = "双手叉腰";
this.poseLibrary[5].Angles = new PoseAngle[4];
this.poseLibrary[5].Angles[0] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 110, 10);
this.poseLibrary[5].Angles[1] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 90, 10);
this.poseLibrary[5].Angles[2] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight,110, 10);
this.poseLibrary[5].Angles[3] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 90, 10);

//第七个手势
this.poseLibrary[6] = new Pose();
this.poseLibrary[6].Title = "两手与身成角";
this.poseLibrary[6].Angles = new PoseAngle[4];
this.poseLibrary[6].Angles[0] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 180, 10);
this.poseLibrary[6].Angles[1] = new PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, JointType.WristLeft, 145, 10);
this.poseLibrary[6].Angles[2] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 180, 10);
this.poseLibrary[6].Angles[3] = new PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, JointType.WristRight, 145, 10);
}

private bool IsPose1(Body body,Pose pose)//判断是否和指定的姿势的匹配
{
bool isPose = true;
double angle1;
double poseAngle;
double poseThreshold;
double highAngle;
double lowAngle;
for(int i=0;i<pose.Angles.Length&&isPose;i=i+2)
{
poseAngle = pose.Angles[i].Angle;
poseThreshold = pose.Angles[i].Threshold;
angle1 = GetJointAngle1(body.Joints[pose.Angles[i].StartJoint], body.Joints[pose.Angles[i].AngleJoint], body.Joints[pose.Angles[i].EndJoint]);
//关节角度的一个阈值范围
highAngle = poseAngle + poseThreshold;
lowAngle = poseAngle - poseThreshold;
//if语句用来判断角度是否在360度范围内，如果不在，则转换到该范围内。
if ( highAngle >= 180 || lowAngle < 0)
{
lowAngle = (lowAngle < 0) ? 360 + lowAngle : lowAngle;
highAngle = highAngle % 360;

isPose = !(lowAngle > angle1 && angle1 > highAngle);
}
else
{
isPose = (lowAngle <= angle1 && highAngle >= angle1);
}
}
return isPose;
}

private bool IsPose2(Body body,Pose pose)
{
bool isPose = true;
double angle2;
double poseAngle;
double poseThreshold;
double highAngle;
double lowAngle;
for (int i = 1; i < pose.Angles.Length && isPose; i = i + 2)
{
poseAngle = pose.Angles[i].Angle;
poseThreshold = pose.Angles[i].Threshold;
angle2 = GetJointAngle2(body.Joints[pose.Angles[i].StartJoint], body.Joints[pose.Angles[i].AngleJoint], body.Joints[pose.Angles[i].EndJoint]);
//关节角度的一个阈值范围
highAngle = poseAngle + poseThreshold;
lowAngle = poseAngle - poseThreshold;
//if语句用来判断角度是否在360度范围内，如果不在，则转换到该范围内。
if (highAngle >= 180 || lowAngle < 0)
{
lowAngle = (lowAngle < 0) ? 360 + lowAngle : lowAngle;
highAngle = highAngle % 360;

isPose = !(lowAngle > angle2 && angle2> highAngle);
}
else
{
isPose = (lowAngle <= angle2 && highAngle >= angle2);
}
}
return isPose;
}

private void ProcessPosePerforming(Body body)//处理训练者的姿势
{
if (IsPose1(body, this.poseLibrary[0]) && IsPose2(body, this.poseLibrary[0]))
{
Console.WriteLine("左手举起匹配");
textBox1.Text = "姿势一匹配";
imgBrush.ImageSource = new BitmapImage(new Uri(@"F:\kinetc2资料\骨骼和彩色叠加\WpfApplication1\WpfApplication1\Image\1.png", UriKind.Relative));
ellipseWithImageBrush.Fill = imgBrush;
}
else if (IsPose1(body, this.poseLibrary[1]) && IsPose2(body, this.poseLibrary[1]))
{
Console.WriteLine("左手放在胸前");
textBox1.Text = "姿势二匹配";
imgBrush.ImageSource = new BitmapImage(new Uri(@"F:\kinetc2资料\骨骼和彩色叠加\WpfApplication1\WpfApplication1\Image\2.png", UriKind.Relative));
ellipseWithImageBrush.Fill = imgBrush;
}
else if (IsPose1(body, this.poseLibrary[2]) && IsPose2(body, this.poseLibrary[2]))
{
Console.WriteLine("左手放在胸前");
textBox1.Text = "姿势三匹配";
imgBrush.ImageSource = new BitmapImage(new Uri(@"F:\kinetc2资料\骨骼和彩色叠加\WpfApplication1\WpfApplication1\Image\3.png", UriKind.Relative));
ellipseWithImageBrush.Fill = imgBrush;
}
else if (IsPose1(body, this.poseLibrary[3]) && IsPose2(body, this.poseLibrary[3]))
{
Console.WriteLine("左手放在胸前");
textBox1.Text = "姿势四匹配";
imgBrush.ImageSource = new BitmapImage(new Uri(@"F:\kinetc2资料\骨骼和彩色叠加\WpfApplication1\WpfApplication1\Image\4.png", UriKind.Relative));
ellipseWithImageBrush.Fill = imgBrush;
}
else if (IsPose1(body, this.poseLibrary[4]) && IsPose2(body, this.poseLibrary[4]))
{
Console.WriteLine("左手放在胸前");
textBox1.Text = "姿势五匹配";
imgBrush.ImageSource = new BitmapImage(new Uri(@"F:\kinetc2资料\骨骼和彩色叠加\WpfApplication1\WpfApplication1\Image\5.png", UriKind.Relative));
ellipseWithImageBrush.Fill = imgBrush;
}
else if (IsPose1(body, this.poseLibrary[5]) && IsPose2(body, this.poseLibrary[5]))
{
Console.WriteLine("左手放在胸前");
textBox1.Text = "姿势六匹配";
imgBrush.ImageSource = new BitmapImage(new Uri(@"F:\kinetc2资料\骨骼和彩色叠加\WpfApplication1\WpfApplication1\Image\6.png", UriKind.Relative));
ellipseWithImageBrush.Fill = imgBrush;
}
else if (IsPose1(body, this.poseLibrary[6]) && IsPose2(body, this.poseLibrary[6]))
{
Console.WriteLine("左手放在胸前");
textBox1.Text = "姿势七匹配";
imgBrush.ImageSource = new BitmapImage(new Uri(@"F:\kinetc2资料\骨骼和彩色叠加\WpfApplication1\WpfApplication1\Image\7.png", UriKind.Relative));
ellipseWithImageBrush.Fill = imgBrush;
}
else
{
textBox1.Text = null;
imgBrush.ImageSource = new BitmapImage(new Uri(@"F:\kinetc2资料\骨骼和彩色叠加\WpfApplication1\WpfApplication1\Image\test.png", UriKind.Relative));
ellipseWithImageBrush.Fill = imgBrush;
}
}

//获得关节点在屏幕上的坐标
private Point GetJointPointScreen(Joint oneJoint)
{
//骨骼坐标转化为深度图像坐标
DepthSpacePoint depthPoint = this.kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(oneJoint.Position);

//深度图像坐标转化为屏幕坐标

depthPoint.X = (int)((depthPoint.X * imageCoordinate.Width) / 512);
depthPoint.Y = (int)((depthPoint.Y * imageCoordinate.Height) / 424);

//返回Point类型变量
return new Point(depthPoint.X, depthPoint.Y);
}

public ImageSource BodyImageSource
{
get
{
return this.imageSource;
}
}
public ImageSource ColorImageSource
{
get
{
return this.colorBitmap;
}
}
private void Window_Closed(object sender, EventArgs e)
{
if (this.multiFrameSourceReader != null)
{
this.multiFrameSourceReader.Dispose();
this.multiFrameSourceReader = null;
}
if (this.kinectSensor != null)
{
this.kinectSensor.Close();
this.kinectSensor = null;
}
}

public void SaveAllJoints(Body[] bodies)
{
FileStream fs = new FileStream("F:\\SaveAllJoints.txt", FileMode.OpenOrCreate);
StreamWriter sw = new StreamWriter(fs);
foreach (Body x in bodies)
{
for (int i = 0; i < 25; i++)
{
// The position of the joint in camera space.摄像机坐标系下的坐标
sw.WriteLine("{0}[{1},{2},{3}]", (JointType)i, x.Joints[(JointType)i].Position.X, x.Joints[(JointType)i].Position.Y, x.Joints[(JointType)i].Position.Z);
}
sw.Write("\r\n");
}
sw.Flush();
sw.Close();
fs.Close();
}

private void SaveAll_Click(object sender, RoutedEventArgs e)
{
this.Savejudge = true;
}

}
}