private double[,] ComputeTransformation(SerializableBody messageBody, SerializableBody entryBody, Dictionary <Joint, Joint> matching) { if (entryBody == null || messageBody == null) { if (DEBUG >= 1) { Console.WriteLine("Transform unsuccessful - null value"); } return(null); } JointType[] jointTypes = new JointType[] { JointType.Neck, JointType.SpineMid, JointType.ShoulderLeft, JointType.ShoulderRight }; foreach (JointType jointType in jointTypes) { if (!matching.ContainsKey(messageBody.Joints[jointType])) { matching.Add(messageBody.Joints[jointType], entryBody.Joints[jointType]); } } double[,] matrixCoeff = SolveMatrix(matching); return(matrixCoeff); }
public MovingState IsMoving(Body body, SerializableBody lastBody) { if (lastBody == null || !body.IsTracked) { return(MovingState.Moving); } foreach (JointType jointType in importantJoints) { if (body.Joints[jointType].TrackingState != TrackingState.Tracked) { return(MovingState.Moving); } } float diffSquareDistance = 0; foreach (KeyValuePair <JointType, Joint> entry in body.Joints) { diffSquareDistance += EuclideanSquareDistance(entry.Value, lastBody.Joints[entry.Key]); } Console.WriteLine("Moving Diff: " + diffSquareDistance); if (diffSquareDistance <= MOVING_DISTANCE_EPSILON) { return(MovingState.Stand); } else { return(MovingState.Moving); } }
// TODO Set just for gesture body! - Done private double[,] PrepareTransform(string srcAddress, SerializableBody srcBody, string tgtAddress, SerializableBody tgtBody) { double[,] matrixCoeff; foreach (TopologyEdge edge in Topology) { if (edge.SourceIPAddress.Equals(srcAddress) && edge.TargetIPAddress.Equals(tgtAddress)) { matrixCoeff = this.ComputeTransformation(srcBody, tgtBody, edge.Matching); edge.TransformMatrixCoefficients = matrixCoeff; return(matrixCoeff); } else if (edge.SourceIPAddress.Equals(tgtAddress) && edge.TargetIPAddress.Equals(srcAddress)) { matrixCoeff = this.ComputeTransformation(tgtBody, srcBody, edge.Matching); edge.TransformMatrixCoefficients = matrixCoeff; return(matrixCoeff); } } Dictionary <Joint, Joint> matching = new Dictionary <Joint, Joint>(); matrixCoeff = this.ComputeTransformation(srcBody, tgtBody, matching); Topology.Add(new TopologyEdge(srcAddress, tgtAddress, matching, matrixCoeff)); // find graph center this.FindGraphCenter(); return(matrixCoeff); }
private void TransformBodies(SerializableBody[] bodies, double[,] matrixCoeff, string sourceAddress) { SerializableBody[] transformedBodies = new SerializableBody[bodies.Length]; for (int i = 0; i < transformedBodies.Length; i++) { if (bodies[i].IsTracked) { transformedBodies[i] = TransformBody(bodies[i], matrixCoeff); } } this.BodiesDictionary.AddOrUpdate(sourceAddress, transformedBodies, (k, v) => transformedBodies); }
private MessageData PrepareData(Body[] bodies) { MessageData message = new MessageData(); message.MessageType = SharedData.Type.Data; message.SrcIPAddress = sender.LocalEndPoint.ToString(); message.Timestamp = DateTime.Now.Add(timestampDiff); SerializableBody[] serializableBodies = new SerializableBody[bodies.Length]; for (int i = 0; i < bodies.Length; i++) { serializableBodies[i] = PrepareBody(bodies[i], (lastBodies == null || lastBodies.Length <= i) ? null : lastBodies[i]); } message.Bodies = serializableBodies; lastBodies = serializableBodies; return(message); }
public SerializableBody TransformBody(SerializableBody body, double[,] coefficients) { SerializableBody transformedBody = body.Clone(); Dictionary <JointType, Joint> joints = new Dictionary <JointType, Joint>(); foreach (KeyValuePair <JointType, Joint> entry in body.Joints) { joints.Add(entry.Key, TransformJoint(entry.Value, coefficients)); } IReadOnlyDictionary <JointType, Joint> transformedJoints = joints; transformedBody.Joints = transformedJoints; return(transformedBody); }
private SerializableBody PrepareBody(Body body, SerializableBody lastBody) { MovingState movingState = IsMoving(body, lastBody); CalibrationState calibrationState = CalibrationState.Uncalibrated; RaisedHand raisedHand = RaisedHand.None; bool gestureDetected = false; if (lastBody != null) { switch (lastBody.CalibrationState) { // raise hand case CalibrationState.Uncalibrated: gestureDetected = false; raisedHand = IsRaisedHand(body); calibrationState = (raisedHand == RaisedHand.None) ? CalibrationState.Uncalibrated : CalibrationState.RaisedHand; break; // wait to stand case CalibrationState.RaisedHand: if (lastBody.GestureDetected) { gestureDetected = false; calibrationState = CalibrationState.SavedPose; raisedHand = lastBody.RaisedHand; break; } if (movingState == MovingState.Stand) { gestureDetected = true; calibrationState = CalibrationState.RaisedHand; raisedHand = lastBody.RaisedHand; // show notification this.mainWindow.Dispatcher.Invoke( new Action(() => { Storyboard sb = this.mainWindow.notificationEllipse.FindResource("notificationStoryBoard") as Storyboard; sb.Begin(); })); } else { gestureDetected = false; raisedHand = IsRaisedHand(body); calibrationState = (raisedHand == RaisedHand.None) ? CalibrationState.Uncalibrated : CalibrationState.RaisedHand; } break; // lower hand case CalibrationState.SavedPose: gestureDetected = false; raisedHand = IsRaisedHand(body); calibrationState = (raisedHand == RaisedHand.None) ? CalibrationState.Uncalibrated : CalibrationState.SavedPose; break; } } return(new SerializableBody(body, movingState, calibrationState, raisedHand, gestureDetected)); }