我想通过使用从 Kinect 获取的骨骼的 X、Y 和 Z 坐标来创建一个 C# wcf 程序来跟踪人的跌倒。直到得到坐标，我才成功。但是当我使用算法计算人的运动速度时，程序停止工作。这是我的代码。请帮我找出错误。
这是我的代码；
public partial class MainWindow : Window
{
KinectSensor sensor = KinectSensor.KinectSensors[0];
Skeleton[] totalSkeleton = new Skeleton[6];
private static Skeleton[] data;
Skeleton skele;
#region "Variables"
private const double BodyCenterThickness = 10;
private const double ClipBoundsThickness = 10;
private readonly Brush centerPointBrush = Brushes.Blue;
private readonly Brush trackedJointBrush = new SolidColorBrush(Color.FromArgb(255, 68, 192, 68));
private readonly Brush inferredJointBrush = Brushes.Yellow;
private readonly Pen trackedBonePen = new Pen(Brushes.Green, 6);
private readonly Pen inferredBonePen = new Pen(Brushes.Gray, 1);
private DrawingImage imageSource;
private const double JointThickness = 3;
private DrawingGroup drawingGroup;
private const float RenderWidth = 640.0f;
private const float RenderHeight = 480.0f;
#endregion
public MainWindow()
{
InitializeComponent();
Loaded += MainWindow_Loaded;
//After Initialization subscribe to the unloaded event of the form
//We use this event to stop the sensor when the application is being closed.
Unloaded += MainWindow_Unloaded;
}
void MainWindow_Unloaded(object sender, RoutedEventArgs e)
{
//stop the Sestor
sensor.Stop();
}
void MainWindow_Loaded(object sender, RoutedEventArgs e)
{ //Create a Drawing Group that will be used for Drawing
this.drawingGroup = new DrawingGroup();
//Create an image Source that will display our skeleton
this.imageSource = new DrawingImage(this.drawingGroup);
//Display the Image in our Image control
Image.Source = imageSource;
try
{
//Check if the Sensor is Connected
if (sensor.Status == KinectStatus.Connected)
{
//Start the Sensor
sensor.Start();
//Tell Kinect Sensor to use the Default Mode(Human Skeleton Standing) || Seated(Human Skeleton Sitting Down)
sensor.SkeletonStream.TrackingMode = SkeletonTrackingMode.Default;
//Subscribe to te Sensor's SkeletonFrameready event to track the joins and create the joins to display on our image control
sensor.SkeletonFrameReady += sensor_SkeletonFrameReady;
//nice message with Colors to alert you if your sensor is working or not
Message.Text = "Kinect Ready";
Message.Background = new SolidColorBrush(Colors.Green);
Message.Foreground = new SolidColorBrush(Colors.White);
// Skeleton firstSkeleton = (from trackskeleton in totalSkeleton where trackskeleton.TrackingState == SkeletonTrackingState.
//// Tracked
//select trackskeleton).FirstOrDefault();
// this.MapJointsWithUIElement(firstSkeleton);
// Turn on the skeleton stream to receive skeleton frames
this.sensor.SkeletonStream.Enable();
}
else if (sensor.Status == KinectStatus.Disconnected)
{
//nice message with Colors to alert you if your sensor is working or not
Message.Text = "Kinect Sensor is not Connected";
Message.Background = new SolidColorBrush(Colors.Orange);
Message.Foreground = new SolidColorBrush(Colors.Black);
}
else if (sensor.Status == KinectStatus.NotPowered)
{
//nice message with Colors to alert you if your sensor is working or not
Message.Text = "Kinect Sensor is not Powered";
Message.Background = new SolidColorBrush(Colors.Red);
Message.Foreground = new SolidColorBrush(Colors.Black);
}
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
}
}
private void MapJointsWithUIElement(Skeleton skeleton)
{
Point mappedPoint = this.ScalePosition(skeleton.Joints[JointType.HandRight].Position);
//Point mappedPointw = this.ScalePosition(skeleton.Joints[JointType.WristRight].Position);
myhandPosition.Content = string.Format("X:{0},Y:{1}", mappedPoint.X, mappedPoint.Y);
//myhandPosition.Content = string.Format("X:{0},Y:{1}", mappedPoint.X, mappedPoint.Y);
Canvas.SetLeft(righthand, mappedPoint.X);
Canvas.SetTop(righthand, mappedPoint.Y);
//float A = skeletonFrame.FloorClipPlane.Item1;
}
public void verificaCaduta(SkeletonFrame skeletonFrame, Skeleton skeleton)
{
if (skeletonFrame != null)
{
}
}
void sensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
//declare an array of Skeletons
Skeleton[] skeletons = new Skeleton[1];
Skeleton firstSkeleton;
Skeleton[] data;
// float A = skeletonFrame.FloorClipPlane.Item1;
//Opens a SkeletonFrame object, which contains one frame of skeleton data.
using (SkeletonFrame skeletonframe = e.OpenSkeletonFrame())
{
//Check if the Frame is Indeed open
if (skeletonframe != null)
{
Skeleton[] skeletonData = new Skeleton[skeletonframe.SkeletonArrayLength];
skeletonframe.CopySkeletonDataTo(skeletonData);
// Copies skeleton data to an array of Skeletons, where each Skeleton contains a collection of the joints.
//draw the Skeleton based on the Default Mode(Standing), "Seated"
//skeletonframe.CopySkeletonDataTo(skeletons);
if (sensor.SkeletonStream.TrackingMode == SkeletonTrackingMode.Default)
{
//Skeleton firstSkeleton = (from trackskeleton in skeletons
//where trackskeleton.TrackingState == SkeletonTrackingState.Tracked
Skeleton playerSkeleton = (from s in skeletonData where s.TrackingState == SkeletonTrackingState.Tracked select s).FirstOrDefault();
DrawStandingSkeletons(skeletonData);
// float x = playerSkeleton.Position.X;
//Skeleton firstSkeleton;
//DrawingContext drawingContext;
//Joint rightHand = playerSkeleton.Joints[JointType.HandRight];
// skeletonframe.CopySkeletonDataTo(skeletonData);
//float A = skeletonframe.FloorClipPlane.Item1;
//float B = skeletonframe.FloorClipPlane.Item2;
//float C = skeletonframe.FloorClipPlane.Item3;
//float D = skeletonframe.FloorClipPlane.Item4;
//float x = rightHand.Position.X;
//float y = rightHand.Position.Y;
//float z = rightHand.Position.Z;
//float powA = A * A;
//float powB = B * B;
//float powC = C * C;
//float numerater = x + y + z + D;
//float denominater = powA + powB + powC;
//float distance = numerater / (float)System.Math.Sqrt(denominater);
//// Console.WriteLine("probability " + distance);
//if (distance <= 0.60)
//{
// MessageBox.Show("fall detection");
//}
}
//else if (sensor.SkeletonStream.TrackingMode == SkeletonTrackingMode.Seated)
//{
// Draw a Seated Skeleton with 10 joints
// DrawSeatedSkeletons(skeletons);
// skeletonframe.CopySkeletonDataTo(skeletons);
//}
}
}
}
private void DrawStandingSkeletons(Skeleton[] skeletons)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
//Draw a Transparent background to set the render size or our Canvas
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight));
//If the skeleton Array has items
if (skeletons.Length != 0)
{
//Loop through the Skeleton joins
foreach (Skeleton skel in skeletons)
{
RenderClippedEdges(skel, dc);
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
this.DrawBonesAndJoints(skel, dc);
}
else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
{
dc.DrawEllipse(this.centerPointBrush, null, this.SkeletonPointToScreen(skel.Position),
BodyCenterThickness, BodyCenterThickness);
}
}
}
//Prevent Drawing outside the canvas
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
}
private void DrawSeatedSkeletons(Skeleton[] 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);
}
else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
{
dc.DrawEllipse(this.centerPointBrush, null, this.SkeletonPointToScreen(skel.Position), BodyCenterThickness, BodyCenterThickness);
}
}
}
//Prevent Drawing outside the canvas
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
}
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));
}
}
public void DrawBonesAndJoints(Skeleton skeleton, DrawingContext drawingContext)
{
float x1, y, z;
// 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);
Message2.Text = joint.Position.X.ToString();
Message4.Text = joint.Position.Y.ToString();
//if (joint.Position.X > 0.4)
//{
// Message3.Text = "fall";
//}
//dovrei calcolare la distanza dal pavimento
//joint head
//float addendo1 = A * firstSkeleton.Joints[JointType.Head].Position.X;
x1 = joint.Position.X;
y = joint.Position.Y;
z = joint.Position.Z;
//float addendo1 = A * skeleton.Position.X;
//float addendo2 = B * skeleton.Position.Y;
//float addendo3 = C * skeleton.Position.Z;
//float addendo1_d = A * A;
//float addendo2_d = B * B;
//float addendo3_d = C * C;
//float numeratore = addendo1 + addendo2 + addendo3 + D;
//float denominatore = addendo1_d + addendo2_d + addendo3_d;
//float distanza = numeratore / (float)System.Math.Sqrt(denominatore);
//// Console.WriteLine("probabile caduta " + distanza);
//if (distanza <= 0.60)
//{
// Message3.Text="fall detection";
//}
}
// joint.Position.Z
}
}
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));
}
/// <summary>
/// Maps a SkeletonPoint to lie within our render space and converts to Point
/// </summary>
/// <param name="skelpoint">point to map</param>
/// <returns>mapped point</returns>
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 Window_Loaded(object sender, RoutedEventArgs e)
{
}
private Point ScalePosition(SkeletonPoint skeletonPoint)
{
DepthImagePoint depthPoint = this.sensor.CoordinateMapper.
MapSkeletonPointToDepthPoint(skeletonPoint, DepthImageFormat.
Resolution640x480Fps30);
return new Point(depthPoint.X, depthPoint.Y);
//textBox1.Text = depthPoint.X.ToString();
}
}
}