private void Reset(Z3Body body) { this.CurrentStep = 0; //this.StepLastPercentage = 0; this.UpdateTargetBody(body); this.AccumulatedError = 0; this.Target = null; }
private void Reset(Z3Body body) { this.CurrentStep = 0; this.LastDistance = 0; this.UpdateTargetBody(body); this.AccumulatedError = 0; this.Target = null; }
public Z3Target CalcNearestTargetBody(Z3Body startBody) { // Create binary search to look for nearest body int numSteps = 6; int angleThreshold = 90; int angleIncrement = 90; Z3Target target = null; for (int i = 0; i < numSteps; ++i) { // Ask for a witness which is within the range target = Z3AnalysisInterface.GenerateTarget( this.Transform, this.Restriction, startBody, angleThreshold); // Update angle threshold angleIncrement /= 2; if (target != null) { angleThreshold -= angleIncrement; } else { angleThreshold += angleIncrement; } } // If target still null it probably means Z3 was unable to solve the restrictions // This way we generate a target using onlye the transforms if (target == null) { target = new Z3Target(); target.Body = this.Transform.Transform(startBody); target.TransformedJoints = this.Transform.GetJointTypes(); } // If target still null assing a new body as an error proof policy if (target == null) { target = new Z3Target(); target.Body = startBody; } return(target); }
// Generates a body witness which satisfies two conditions // 1. It is within a range (angle threshold) of a transform from a start body // 2. It is within the considered restrictions public static Z3Target GenerateTarget( BodyTransform transform, CompositeBodyRestriction restriction, Z3Body startBody, int angleThreshold) { var z3ConstBody = Z3Body.MkZ3Const(); z3ConstBody.Norms = startBody.Norms; var transformedBody = transform.Transform(startBody); var joints = transform.GetJointTypes().Union(restriction.GetJointTypes()).ToList(); var isNearExpr = z3ConstBody.IsAngleBetweenLessThan(transformedBody, joints, angleThreshold); var evaluateExpr = restriction.Evaluate(z3ConstBody); //var normsExpr = BodyRestrictionBuilder.EvaluateNorms(startBody, z3ConstBody); //var expr = Z3.Context.MkAnd(isNearExpr, evaluateExpr, normsExpr); //var expr = Z3.Context.MkAnd(evaluateExpr, normsExpr); var expr = Z3.Context.MkAnd(evaluateExpr, isNearExpr); var checkResult = CheckStatus(expr); if (checkResult.Status == Status.SATISFIABLE) { var witness = CreateBodyWitness( z3ConstBody, checkResult.Model, restriction.GetJointTypes(), startBody); var target = new Z3Target(); target.Body = witness; target.RestrictedJoints = restriction.GetJointTypes(); target.TransformedJoints = transform.GetJointTypes(); foreach (var jointType in transform.GetJointTypes()) { target.Body.Joints[jointType] = transformedBody.Joints[jointType]; } return(target); } else { return(null); } }
/// <summary> /// This maps a Z3 body to Kinect joints. /// Inverse of the method above. /// Does coordinate mapping as well. /// </summary> /// <param name="baseJoints"></param> /// <param name="target"></param> /// <returns></returns> public static Dictionary<Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> CreateKinectJoints( IReadOnlyDictionary<Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> baseJoints, Z3Target target) { // Acquire all vectors from Z3 target var jointVectors = new Dictionary<Microsoft.Kinect.JointType, Vector3D>(); // Spine base is the root of the system, as it has no direction to store, it stores its own position jointVectors.Add( Microsoft.Kinect.JointType.SpineBase, new Vector3D( baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.X, baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.Y, baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.Z)); var z3JointTypes = EnumUtil.GetValues<PreposeGestures.JointType>(); var targetJoints = target.TransformedJoints; foreach (var jointType in z3JointTypes) { if (jointType != PreposeGestures.JointType.SpineBase) { // If target has calculated the joint add target jointType if (targetJoints.Contains(jointType)) { AddZ3JointToVectors3D(target.Body, baseJoints, jointVectors, jointType); } // Or else use the joint from current Kinect data (baseJoints) else { jointVectors.Add( (Microsoft.Kinect.JointType)jointType, new Vector3D( baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X, baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y, baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z)); } } } var result = HipsSpineToKinectCoordinateSystem(jointVectors, baseJoints, targetJoints); return result; }
CreateKinectJoints( IReadOnlyDictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> baseJoints, Z3Target target) { // Calc base rotation matrix var rotationMatrix = new Matrix3D(); InitMatrix(out rotationMatrix, baseJoints); // Acquire all vectors from Z3 target var jointVectors = new Dictionary <Microsoft.Kinect.JointType, Vector3D>(); // Spine base is the root of the system, as it has no direction to store, it stores its own position jointVectors.Add( Microsoft.Kinect.JointType.SpineBase, new Vector3D( baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.X, baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.Y, baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.Z)); var z3JointTypes = EnumUtil.GetValues <PreposeGestures.JointType>(); var targetJoints = target.TransformedJoints; foreach (var jointType in z3JointTypes) { if (jointType != PreposeGestures.JointType.SpineBase) { // If target has calculated the joint add target jointType if (targetJoints.Contains(jointType)) { AddZ3JointToVectors3D(target.Body, baseJoints, jointVectors, jointType); } // Or else use the joint from current Kinect data (baseJoints) else { jointVectors.Add( (Microsoft.Kinect.JointType)jointType, new Vector3D( baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X, baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y, baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z)); } } } // Rotate all vectors to the current base body coordinate system var kinectJointTypes = EnumUtil.GetValues <Microsoft.Kinect.JointType>(); foreach (var jointType in kinectJointTypes) { if (jointType != Microsoft.Kinect.JointType.SpineBase && targetJoints.Contains((PreposeGestures.JointType)jointType)) { jointVectors[jointType] *= rotationMatrix; } } // Add base body position (spineBase position) to translate result // The operations order matter in this case SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.SpineMid); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.SpineShoulder); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.Neck); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.Head); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ShoulderLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ElbowLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.WristLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandTipLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ThumbLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ShoulderRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ElbowRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.WristRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandTipRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ThumbRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HipLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.KneeLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.AnkleLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.FootLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HipRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.KneeRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.AnkleRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.FootRight); // Filling the results var result = new Dictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint>(); foreach (var jointType in kinectJointTypes) { var joint = new Joint(); var position = new CameraSpacePoint(); position.X = (float)jointVectors[jointType].X; position.Y = (float)jointVectors[jointType].Y; position.Z = (float)jointVectors[jointType].Z; joint.Position = position; joint.TrackingState = TrackingState.Tracked; result.Add(jointType, joint); } return(result); }
public Z3Target CalcNearestTargetBody(Z3Body startBody) { //Z3Target target = null; //// Create binary search to look for nearest body //int numSteps = 1; //int degreesThreshold = 90; //int degreesIncrement = 90; //for (int i = 0; i < numSteps; ++i) //{ // // Ask for a witness which is within the range // target = Z3AnalysisInterface.GenerateTarget( // this.Transform, // this.Restriction, // startBody, // degreesThreshold); // // Update degrees threshold // degreesIncrement /= 2; // if (target != null) // { // degreesThreshold -= degreesIncrement; // } // else // { // degreesThreshold += degreesIncrement; // } //} //// If target still null it probably means Z3 was unable to solve the restrictions //// This way we generate a target using onlye the transforms //if(target == null) //{ // target = new Z3Target(); // target.Body = this.Transform.Transform(startBody); // target.TransformedJoints = this.Transform.GetJointTypes(); //} //// If target still null assign a new body as an error proof policy //if(target == null) //{ // target = new Z3Target(); // target.Body = startBody; //} //return target; var target = new Z3Target(); target.Body = this.Transform.Transform(startBody); target.TransformedJoints = this.Transform.GetJointTypes(); // If target still null assign a new body as an error proof policy if (target == null) { target = new Z3Target(); target.Body = startBody; } return(target); }
internal void UpdateTargetBody(Z3Body start, Context localZ3Context) { var stopwatch = new Stopwatch(); StatisticsEntrySynthesize synTime = new StatisticsEntrySynthesize(); stopwatch.Start(); var time0 = stopwatch.ElapsedMilliseconds; if (this.Target != null && this.CurrentStep > 0) { // If last target is not null than use the target transformed joints instead of the start transformed joints // This way no error is cumulated along the transforms due to the matching precisions foreach (var jointType in this.Gesture.Steps[this.CurrentStep - 1].Pose.GetTransformJointTypes()) { // The Z3Point3D depends on a specific Z3 context deep underneath // We need to thread the context through start.Joints[jointType] = new Z3Point3D( Z3Math.GetRealValue(this.Target.Body.Joints[jointType].X), Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Y), Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Z), localZ3Context); } } this.Target = this.Gesture.Steps[this.CurrentStep].Pose.CalcNearestTargetBody(start); this.LastDistanceVectors = UpdateDistanceVectors(start, Z3.Context); var time1 = stopwatch.ElapsedMilliseconds - time0; stopwatch.Stop(); synTime.timeMs = time1; synTime.gestureName = this.Gesture.Name; synTime.poseName = this.Gesture.Steps[CurrentStep].Pose.Name; synTime.uid = frameCount; //totalZ3Time += time1; frameCount++; GestureStatistics.synthTimes.Add(synTime); //Console.Write("z3time: " + (totalZ3Time / frameCount) + " \r"); }
public Z3Target CalcNearestTargetBody(Z3Body startBody) { //Z3Target target = null; //// Create binary search to look for nearest body //int numSteps = 1; //int degreesThreshold = 90; //int degreesIncrement = 90; //for (int i = 0; i < numSteps; ++i) //{ // // Ask for a witness which is within the range // target = Z3AnalysisInterface.GenerateTarget( // this.Transform, // this.Restriction, // startBody, // degreesThreshold); // // Update degrees threshold // degreesIncrement /= 2; // if (target != null) // { // degreesThreshold -= degreesIncrement; // } // else // { // degreesThreshold += degreesIncrement; // } //} //// If target still null it probably means Z3 was unable to solve the restrictions //// This way we generate a target using onlye the transforms //if(target == null) //{ // target = new Z3Target(); // target.Body = this.Transform.Transform(startBody); // target.TransformedJoints = this.Transform.GetJointTypes(); //} //// If target still null assign a new body as an error proof policy //if(target == null) //{ // target = new Z3Target(); // target.Body = startBody; //} //return target; var target = new Z3Target(); target.Body = this.Transform.Transform(startBody); target.TransformedJoints = this.Transform.GetJointTypes(); // If target still null assign a new body as an error proof policy if(target == null) { target = new Z3Target(); target.Body = startBody; } return target; }
// Generates a body witness which satisfies two conditions // 1. It is within a range (angle threshold) of a transform from a start body // 2. It is within the considered restrictions public static Z3Target GenerateTarget( BodyTransform transform, CompositeBodyRestriction restriction, Z3Body startBody, int angleThreshold) { var z3ConstBody = Z3Body.MkZ3Const(); z3ConstBody.Norms = startBody.Norms; var transformedBody = transform.Transform(startBody); var joints = transform.GetJointTypes().Union(restriction.GetJointTypes()).ToList(); var isNearExpr = z3ConstBody.IsAngleBetweenLessThan(transformedBody, joints, angleThreshold); var evaluateExpr = restriction.Evaluate(z3ConstBody); //var normsExpr = BodyRestrictionBuilder.EvaluateNorms(startBody, z3ConstBody); //var expr = Z3.Context.MkAnd(isNearExpr, evaluateExpr, normsExpr); //var expr = Z3.Context.MkAnd(evaluateExpr, normsExpr); var expr = Z3.Context.MkAnd(evaluateExpr, isNearExpr); var checkResult = CheckStatus(expr); if (checkResult.Status == Status.SATISFIABLE) { var witness = CreateBodyWitness( z3ConstBody, checkResult.Model, restriction.GetJointTypes(), startBody); var target = new Z3Target(); target.Body = witness; target.RestrictedJoints = restriction.GetJointTypes(); target.TransformedJoints = transform.GetJointTypes(); foreach (var jointType in transform.GetJointTypes()) target.Body.Joints[jointType] = transformedBody.Joints[jointType]; return target; } else { return null; } }
/// <summary> /// This maps a Z3 body to Kinect joints. /// Inverse of the method above. /// Does coordinate mapping as well. /// </summary> /// <param name="baseJoints"></param> /// <param name="target"></param> /// <returns></returns> public static Dictionary<Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> CreateKinectJoints( IReadOnlyDictionary<Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> baseJoints, Z3Target target) { // Calc base rotation matrix var rotationMatrix = new Matrix3D(); InitMatrix(out rotationMatrix, baseJoints); // Acquire all vectors from Z3 target var jointVectors = new Dictionary<Microsoft.Kinect.JointType, Vector3D>(); // Spine base is the root of the system, as it has no direction to store, it stores its own position jointVectors.Add( Microsoft.Kinect.JointType.SpineBase, new Vector3D( baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.X, baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.Y, baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.Z)); var z3JointTypes = EnumUtil.GetValues<PreposeGestures.JointType>(); var targetJoints = target.TransformedJoints; foreach (var jointType in z3JointTypes) { if (jointType != PreposeGestures.JointType.SpineBase) { // If target has calculated the joint add target jointType if (targetJoints.Contains(jointType)) { AddZ3JointToVectors3D(target.Body, baseJoints, jointVectors, jointType); } // Or else use the joint from current Kinect data (baseJoints) else { jointVectors.Add( (Microsoft.Kinect.JointType)jointType, new Vector3D( baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X, baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y, baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z)); } } } // Rotate all vectors to the current base body coordinate system var kinectJointTypes = EnumUtil.GetValues<Microsoft.Kinect.JointType>(); foreach (var jointType in kinectJointTypes) { if (jointType != Microsoft.Kinect.JointType.SpineBase && targetJoints.Contains((PreposeGestures.JointType)jointType)) { jointVectors[jointType] *= rotationMatrix; } } // Add base body position (spineBase position) to translate result // The operations order matter in this case SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.SpineMid); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.SpineShoulder); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.Neck); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.Head); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ShoulderLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ElbowLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.WristLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandTipLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ThumbLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ShoulderRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ElbowRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.WristRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandTipRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ThumbRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HipLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.KneeLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.AnkleLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.FootLeft); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HipRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.KneeRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.AnkleRight); SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.FootRight); // Filling the results var result = new Dictionary<Microsoft.Kinect.JointType, Microsoft.Kinect.Joint>(); foreach (var jointType in kinectJointTypes) { var joint = new Joint(); var position = new CameraSpacePoint(); position.X = (float)jointVectors[jointType].X; position.Y = (float)jointVectors[jointType].Y; position.Z = (float)jointVectors[jointType].Z; joint.Position = position; joint.TrackingState = TrackingState.Tracked; result.Add(jointType, joint); } return result; }
public Z3Target CalcNearestTargetBody(Z3Body startBody) { // Create binary search to look for nearest body int numSteps = 6; int angleThreshold = 90; int angleIncrement = 90; Z3Target target = null; for (int i = 0; i < numSteps; ++i) { // Ask for a witness which is within the range target = Z3AnalysisInterface.GenerateTarget( this.Transform, this.Restriction, startBody, angleThreshold); // Update angle threshold angleIncrement /= 2; if (target != null) { angleThreshold -= angleIncrement; } else { angleThreshold += angleIncrement; } } // If target still null it probably means Z3 was unable to solve the restrictions // This way we generate a target using onlye the transforms if(target == null) { target = new Z3Target(); target.Body = this.Transform.Transform(startBody); target.TransformedJoints = this.Transform.GetJointTypes(); } // If target still null assing a new body as an error proof policy if(target == null) { target = new Z3Target(); target.Body = startBody; } return target; }
private void Reset(Z3Body body) { this.CurrentStep = 0; this.StepLastPercentage = 0; this.UpdateTargetBody(body); this.AccumulatedError = 0; this.Target = null; }
internal void UpdateTargetBody(Z3Body startBody) { // delayed statements are a special case // they can be treated as restrictions or as transformations // in order to the pose to retrieve the composite restrictions and transformations // we must first provide the start body to update its delayed statements // so they are treated first on the update target function this.Gesture.Steps[this.CurrentStep].Pose.UpdateDelayedStatements(startBody); var stopwatch = new Stopwatch(); StatisticsEntrySynthesize synTime = new StatisticsEntrySynthesize(); stopwatch.Start(); var time0 = stopwatch.ElapsedMilliseconds; if (this.Target != null && this.CurrentStep > 0) { // If last target is not null than use the target transformed joints instead of the start transformed joints // This way no error is cumulated along the transforms due to the matching precisions foreach (var jointType in this.Gesture.Steps[this.CurrentStep - 1].Pose.GetTransformJointTypes()) { // The Z3Point3D depends on a specific Z3 context deep underneath // We need to thread the context through startBody.Joints[jointType] = new Z3Point3D( Z3Math.GetRealValue(this.Target.Body.Joints[jointType].X), Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Y), Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Z), Z3.Context); } } this.Target = this.Gesture.Steps[this.CurrentStep].Pose.CalcNearestTargetBody(startBody); this.LastDistanceVectors = UpdateDistanceVectors(startBody, Z3.Context); var time1 = stopwatch.ElapsedMilliseconds - time0; stopwatch.Stop(); synTime.timeMs = time1; synTime.gestureName = this.Gesture.Name; synTime.poseName = this.Gesture.Steps[CurrentStep].Pose.Name; synTime.uid = frameCount; GestureStatistics.synthTimes.Add(synTime); frameCount++; }