internal void Update(Z3Body body)
{
this.Restriction = new CompositeBodyRestriction();
this.Transform = new CompositeBodyTransform();
// treating rotate direction
// for each definition
// if a joint appears once
// restrict
// else
// transform
// the rotate direction can be a transform or a restriction
foreach (var definition in this.Statements)
{
var jointCount = 0;
foreach (var definitionTemp in this.Statements)
{
if (definition.JointType == definitionTemp.JointType)
jointCount++;
}
// if for a single joint there is only one rotate direction
// then the restriction should be applied to give more
// freedom for the user while performing the gesture
if(jointCount == 1)
{
this.Restriction.And(new RotateDirectionRestriction(
definition.JointType, body.Joints[definition.JointType],
definition.Degrees, definition.Direction));
}
// else if for that joint there is more than one rotate direction
// then we create a transform to guarantee that all directions
// are well represented
else if(jointCount > 1)
{
this.Transform = this.Transform.Compose(
new CompositeBodyTransform(definition.JointType,
new RotateJointTransform(definition.Degrees, definition.Direction)));
}
}
}
/// <summary>
/// Matches a Kinect body to a set of gestures within the app.
/// </summary>
/// <param name="kinectJoints">Body representation</param>
/// <param name="jumpToNextPose">synthesizes a new body in correct position if true</param>
/// <returns></returns>
public static List<GestureStatus> TestBody(BodyMatcher matcher,
IReadOnlyDictionary<Microsoft.Kinect.JointType,
Microsoft.Kinect.Joint> kinectJoints,
bool jumpToNextPose = false)
{
// convert Kinect.Body to Z3Body
var body = new Z3Body();
if (!jumpToNextPose)
{
body = Z3KinectConverter.CreateZ3Body(kinectJoints);
}
else
{
//body = GetCopiedBodyValues(this.Gestures[this.GetMostAdvancedGesturesIDs()[0]].GetTarget().Body);
var firstGestureBody = matcher.GetLastGestureTarget().Body;
body = new Z3Body(firstGestureBody);
}
return matcher.TestBody(body);
}
public List<GestureStatus> TestBody(Z3Body body)
{
var result = new List<GestureStatus>();
// InitBody synthesizes the initial pose to go after. Synthesizing a pose is _not_ parallel-safe because the Z3 context isn't safe to share across threads
foreach (var matcher in this.Matchers)
{
matcher.InitBody(body);
}
// Data parallel -- each matcher is independent of each other, so we can use Task Parallel Library to send each matcher to a different core if necessary
// The task parallel library should take care of locking the result array for us...let's hope it doesn't serialize by locking result in the wrong way
/*
Parallel.ForEach(this.Matchers,
matcher => {
Context Z3ThreadLocalContext = new Context(new Dictionary<string, string>() { { "MODEL", "true" }, { "PROOF_MODE", "2" } });
matcher.TestBody(body, this.Precision, Z3ThreadLocalContext);
result.Add(matcher.GetStatus());
});
*/
// Matching itself uses the Z3 context - and that can't be shared across threads!
foreach (var matcher in this.Matchers)
{
matcher.TestBody(body, this.Precision, Z3.Context);
result.Add(matcher.GetStatus());
}
// Check for each matcher to see if it succeeded. If it did, then synthesize a new target position.
foreach (var matcher in this.Matchers)
{
if (matcher.GetStatus().succeededDetection)
{
matcher.UpdateTargetBody(body);
}
}
return result;
}
/// <summary>
/// Matches a Kinect body to a set of gestures within the app.
/// </summary>
/// <param name="kinectJoints">Body representation</param>
/// <param name="jumpToNextPose">synthesizes a new body in correct position if true</param>
/// <returns></returns>
public static List <GestureStatus> TestBody(BodyMatcher matcher,
IReadOnlyDictionary <Microsoft.Kinect.JointType,
Microsoft.Kinect.Joint> kinectJoints,
bool jumpToNextPose = false)
{
// convert Kinect.Body to Z3Body
var body = new Z3Body();
if (!jumpToNextPose)
{
body = Z3KinectConverter.CreateZ3Body(kinectJoints);
}
else
{
//body = GetCopiedBodyValues(this.Gestures[this.GetMostAdvancedGesturesIDs()[0]].GetTarget().Body);
var firstGestureBody = matcher.GetLastGestureTarget().Body;
body = new Z3Body(firstGestureBody);
}
return(matcher.TestBody(body));
}
private static void AddZ3JointToVectors3D(
Z3Body targetBody,
IReadOnlyDictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> baseJoints,
Dictionary <Microsoft.Kinect.JointType, Vector3D> jointVectors,
PreposeGestures.JointType jointType)
{
var vector3D = new Vector3D(
targetBody.Joints[jointType].GetXValue(),
targetBody.Joints[jointType].GetYValue(),
-targetBody.Joints[jointType].GetZValue());
//var norm = Z3Math.GetRealValue(targetBody.Norms[jointType]);
var norm =
new Vector3D(
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.X,
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Y,
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Z).Length;
vector3D = vector3D * norm;
jointVectors.Add((Microsoft.Kinect.JointType)jointType, vector3D);
}
public bool IsBodyAccepted(
Z3Body input)
{
bool result = this.Restriction.IsBodyAccepted(input);
return result;
}
private static void AddZ3JointToVectors3D(
Z3Body targetBody,
IReadOnlyDictionary<Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> baseJoints,
Dictionary<Microsoft.Kinect.JointType, Vector3D> jointVectors,
PreposeGestures.JointType jointType)
{
var vector3D = new Vector3D(
targetBody.Joints[jointType].GetXValue(),
targetBody.Joints[jointType].GetYValue(),
-targetBody.Joints[jointType].GetZValue());
//var norm = Z3Math.GetRealValue(targetBody.Norms[jointType]);
var norm =
new Vector3D(
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.X,
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Y,
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Z).Length;
vector3D = vector3D * norm;
jointVectors.Add((Microsoft.Kinect.JointType)jointType, vector3D);
}
private Dictionary<JointType, Point3D> UpdateDistanceVectors(Z3Body body, Context localContext)
{
var distancesZ3Point3Ds = body.GrabDistancePoint3Ds(this.Target.Body, this.Target.GetAllJointTypes());
var distancesVector3Ds = new Dictionary<JointType, Point3D>();
foreach (var pointZ3 in distancesZ3Point3Ds)
{
distancesVector3Ds.Add(
pointZ3.Key,
new Point3D(
Z3Math.GetRealValue(pointZ3.Value.X),
Z3Math.GetRealValue(pointZ3.Value.Y),
Z3Math.GetRealValue(pointZ3.Value.Z)));
}
return distancesVector3Ds;
}
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 GestureStatus TestBody(Z3Body body, int precision, Context localContext)
{
StatisticsEntryMatch matchStat = new StatisticsEntryMatch();
var stopwatch = new Stopwatch();
stopwatch.Start();
var time0 = stopwatch.ElapsedMilliseconds;
var time1 = stopwatch.ElapsedMilliseconds - time0;
// NOT THREAD SAFE XXX
this.LastDistanceVectors = this.UpdateDistanceVectors(body, localContext);
var time2 = stopwatch.ElapsedMilliseconds - time1;
// Check distance to target transformed joints and if body satisfies restrictions
var distance = 2.0;
if (this.Target.TransformedJoints.Count > 0)
distance = CalcMaxDistance(this.LastDistanceVectors, this.Target.TransformedJoints);
this.AccumulatedError += distance - this.LastDistance;
this.LastDistance = distance;
var time3 = stopwatch.ElapsedMilliseconds - time2;
bool transformSucceeded = distance < TrigonometryHelper.GetDistance(precision);
var time4 = stopwatch.ElapsedMilliseconds - time3;
// NOT THREAD SAFE XXX
bool restrictionSucceeded = this.Gesture.Steps[CurrentStep].Pose.IsBodyAccepted(body);
var time5 = stopwatch.ElapsedMilliseconds - time4;
matchStat.timeMs = time5;
matchStat.gestureName = this.Gesture.Name;
matchStat.uid = frameCount;
matchStat.poseName = this.Gesture.Steps[CurrentStep].Pose.Name;
bool succeeded =
(transformSucceeded || this.Target.TransformedJoints.Count == 0) &&
(restrictionSucceeded || this.Target.RestrictedJoints.Count == 0);
// Overall succeeded represents whether the entire gesture was matched on this test, not just a single pose
bool overallSucceeded = false;
// If body is accepted move to matching the next pose in sequence
if (succeeded)
{
this.CurrentStep += 1;
this.AccumulatedError = 0;
// Check if gesture is completed (all steps are finished)
if (this.CurrentStep >= this.Gesture.Steps.Count)
{
this.Reset(body);
this.CompletedCount += 1;
overallSucceeded = true;
}
this.UpdateTargetBody(body);
}
// If body was not accepted then check if error is higher than threshold
// If accumulated error is too high the gesture is broken
else if (this.AccumulatedError > 1)
{
//gesture.Reset(body);
}
var result = this.GetStatus();
// The VisualGestureBuilder API for DiscreteGestureResult cares about whole gestures,
// not about individual poses. We need to check for this case explicitly.
// We then expose succeededDetection in a DiscreteGestureResult .
result.succeededDetection = overallSucceeded;
// TODO: check the semantics of succeededDetectionFirstFrame in the VisualGestureBuilder API
// We here are assuming that firstFrame means this is the first frame where we successfully detected the gesture
// We are assuming that firstFrame does NOT mean this is the first frame where we started tracking the gesture
if (CompletedCount == 1 & overallSucceeded)
result.succeededDetectionFirstFrame = true;
else
result.succeededDetectionFirstFrame = false;
// TODO: check the semantics of confidence in the VisualGestureBuilder API.
// Here confidence is the same as distance from the target body.
// That is NOT the same as if confidence were a probability that we are correct
// We want to have as close semantics as possible to the VisualGestureBuilder API, so we need to double check this
result.confidence = (float)distance;
var time6 = stopwatch.ElapsedMilliseconds - time5;
frameCount++;
// Record the statistics entry here
GestureStatistics.matchTimes.Add(matchStat);
return result;
}
public void InitBody(Z3Body body)
{
InitBody(body, Z3.Context);
}
// returns the min percentage of completion from all restrictions
internal double CalcMinPercentage(Z3Body body, out string mainRestriction)
{
var percentage = this.Restriction.CalcPercentage(body, out mainRestriction);
return percentage;
}
public bool IsBodyAccepted(Z3Body body)
{
BoolExpr resultExpr = RestrictionFunc(body);
Solver solver = Z3.Context.MkSolver();
solver.Push();
solver.Assert(Z3.Context.MkNot(resultExpr));
Status status = solver.Check();
Statistics stats = solver.Statistics;
bool result = (status == Status.UNSATISFIABLE);
solver.Pop();
return result;
}
//internal static SimpleBodyRestriction TouchRestriction(JointType jointType, JointSide handSide)
//{
// double maxY = TrigonometryHelper.GetSine(5);
// return new TouchBodyRestriction(jointType, handSide);
//}
internal static BoolExpr EvaluateNorms(Z3Body body1, Z3Body body2)
{
double normsThreshold = 0.1;
BoolExpr result = Z3Math.True;
var jointTypes = EnumUtil.GetValues<JointType>();
foreach (var jointType in jointTypes)
{
// Calc the distance between the two norms
ArithExpr distance =
Z3Math.Abs(
Z3Math.Sub(
body1.Norms[jointType],
body2.Norms[jointType]));
// Create the boolean expression to evaluate the distance
result =
Z3.Context.MkAnd(
result,
Z3.Context.MkLt(
distance,
Z3Math.Real(normsThreshold)));
}
return result;
}
public PairwiseConflictException(string message, Gesture gesture1, Gesture gesture2, Z3Body body)
: base(message)
{
this.Witness = body;
this.Gesture1 = gesture1;
this.Gesture2 = gesture2;
}
/// <summary>
/// Checks if the pose is within default safety
/// restrictions when the transform and restrictions
/// are applied.
/// </summary>
/// <returns>True if it's safe</returns>
public static bool IsWithinSafetyRestrictions(Pose pose, out Z3Body witness)
{
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
IBodyRestriction safe = Safety.DefaultSafetyRestriction();
BoolExpr inputSafe = safe.Evaluate(input);
BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);
// Try to generate a unsafe witness using the transform
BoolExpr outputUnsafe = Z3.Context.MkNot(safe.Evaluate(transformed));
// Put together all expressions and search for unsat
BoolExpr expr = Z3.Context.MkAnd(inputSafe, transformedRestricted, outputUnsafe);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);
if (solverResult.Status == Status.SATISFIABLE)
{
//Z3Body
witness =
Z3AnalysisInterface.CreateBodyWitness(
transformed,
solverResult.Model,
pose.GetAllJointTypes(),
JointTypeHelper.CreateDefaultZ3Body());
return false;
}
else if (solverResult.Status == Status.UNKNOWN)
{
//Z3Body
witness = JointTypeHelper.CreateDefaultZ3Body();
return false;
}
else
{
Contract.Assert(solverResult.Status == Status.UNSATISFIABLE);
witness = null;
return true;
}
}
void myBodyReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
if (this.IsPaused)
return;
PreposeGesturesFrame retFrame = null;
PreposeGesturesFrameArrivedEventArgs upArgs = new PreposeGesturesFrameArrivedEventArgs();
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
// Perform the gesture matching on this frame
var z3body = new Z3Body();
bodyFrame.GetAndRefreshBodyData(this.bodies);
foreach (var body in this.bodies)
{
if (body.TrackingId == this.mySource.TrackingId)
{
// We are at the correct body - go ahead and feed it to the BodyMatcher
IReadOnlyDictionary<Microsoft.Kinect.JointType, Joint> joints = body.Joints;
z3body = Z3KinectConverter.CreateZ3Body(joints);
var result = this.PreposeGesturesFrameSource.myMatcher.TestBody(z3body);
// Fill in the gesture results for this frame
retFrame = new PreposeGesturesFrame(result, this.mySource, body.TrackingId, bodyFrame.RelativeTime);
break;
}
}
}
}
// TODO: revisit the way the PreposeGesturesFrameReference is implemented to avoid keeping around massive amounts of frames
PreposeGesturesFrameReference retFrameReference = new PreposeGesturesFrameReference(retFrame);
upArgs.FrameReference = retFrameReference;
// Signal that we have a new PreposeGesturesFrame arrived
FrameArrived(this, upArgs);
}
public bool IsTransformedBodyAccepted(Z3Body body)
{
bool result = false;
Z3Body transformedBody = this.mTransform.Transform(body);
result = this.IsBodyAccepted(transformedBody);
return result;
}
public void UpdateDelayedStatements(Z3Body startBody)
{
this.Delayed.Update(startBody);
}
/// <summary>
/// Apply this transform to obtain an output body.
/// </summary>
/// <param name="inputBody"></param>
/// <returns></returns>
public Z3Body Transform(Z3Body inputBody)
{
var outputBody = new Z3Body();
var jointTypes = EnumUtil.GetValues<JointType>();
foreach (var jointType in jointTypes)
{
// Both this and inputBody have the jointType, so transform the joint
if (this.JointTransforms.ContainsKey(jointType) &&
inputBody.Joints.ContainsKey(jointType))
{
outputBody.Joints.Add(jointType,
this.JointTransforms[jointType].Transform(
inputBody.Joints[jointType]));
outputBody.Norms.Add(jointType,
inputBody.Norms[jointType]);
}
// The joint exists in body but there is no transform for it, so add it
else if (inputBody.Joints.ContainsKey(jointType))
{
outputBody.Joints.Add(jointType,
inputBody.Joints[jointType]);
outputBody.Norms.Add(jointType,
inputBody.Norms[jointType]);
}
// Do nothing otherwise
}
return outputBody;
}
public Z3Target()
{
this.Body = new Z3Body();
this.TransformedJoints = new List<JointType>();
this.RestrictedJoints = new List<JointType>();
}
public void FinalResult(Z3Body input, out Z3Body transformed, out BoolExpr evaluation)
{
transformed = input;
evaluation = Z3Math.True;
foreach (var step in this.Steps)
{
var pose = step.Pose;
transformed = pose.Transform.Transform(transformed);
evaluation = Z3.Context.MkAnd(evaluation, pose.Restriction.Evaluate(transformed));
}
}
public void InitBody(Z3Body body, Context localContext)
{
GestureMatcherLocalContext = localContext;
if (this.Target == null)
{
this.UpdateTargetBody(body);
}
}
public static Z3Body CreateBodyWitness(
Z3Body z3ConstCheckedBody,
Model model,
List<JointType> evaluatedJoints,
Z3Body defaultBody)
{
var witness = new Z3Body();
var jointTypes = EnumUtil.GetValues<JointType>();
foreach (var jointType in jointTypes)
{
if (evaluatedJoints.Contains(jointType))
{
var joint = new Z3Point3D(
model.Evaluate(z3ConstCheckedBody.Joints[jointType].X, true) as ArithExpr,
model.Evaluate(z3ConstCheckedBody.Joints[jointType].Y, true) as ArithExpr,
model.Evaluate(z3ConstCheckedBody.Joints[jointType].Z, true) as ArithExpr);
witness.Joints.Add(jointType, joint);
var norm = model.Evaluate(z3ConstCheckedBody.Norms[jointType]) as ArithExpr;
// Check if norm is still an app (meaning it can be anything), then set it to be the default norm
if (norm.ASTKind == Z3_ast_kind.Z3_APP_AST)
witness.Norms.Add(jointType, defaultBody.Norms[jointType]);
else
witness.Norms.Add(jointType, norm);
}
else
{
witness.Joints.Add(jointType, defaultBody.Joints[jointType]);
witness.Norms.Add(jointType, defaultBody.Norms[jointType]);
}
}
return witness;
}
internal void UpdateTargetBody(Z3Body start)
{
UpdateTargetBody(start, Z3.Context);
}
// 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;
}
}
private void Reset(Z3Body body)
{
this.CurrentStep = 0;
this.LastDistance = 0;
this.UpdateTargetBody(body);
this.AccumulatedError = 0;
this.Target = null;
}
public BoolExpr Evaluate(Z3Body body)
{
return this.RestrictionFunc(body);
}
public PoseSafetyException(string message, Pose pose, Z3Body body)
: base(message)
{
this.Body = body;
this.Pose = pose;
}
public double Percentage(Z3Body body)
{
return this.PercentageFunc(body);
}
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;
}
public bool IsBodyAccepted(Z3Body body, Context localContext)
{
BoolExpr resultExpr = Evaluate(body);
bool result = Z3.EvaluateBoolExpr(resultExpr, localContext);
return result;
}
