コード例 #1
0
ファイル: Z3Body.cs プロジェクト: matheusberger/prepose
        public Z3Body(Z3Body baseBody) : this()
        {
            foreach (var jointType in baseBody.Joints.Keys)
            {
                this.Positions.Add(jointType,
                                   new Point3D(
                                       baseBody.Positions[jointType].X,
                                       baseBody.Positions[jointType].Y,
                                       baseBody.Positions[jointType].Z));

                this.Vectors.Add(jointType,
                                 new Point3D(
                                     baseBody.Vectors[jointType].X,
                                     baseBody.Vectors[jointType].Y,
                                     baseBody.Vectors[jointType].Z));

                this.Joints.Add(jointType,
                                new Z3Point3D(
                                    Z3Math.GetRealValue(baseBody.Joints[jointType].X),
                                    Z3Math.GetRealValue(baseBody.Joints[jointType].Y),
                                    Z3Math.GetRealValue(baseBody.Joints[jointType].Z)
                                    ));

                this.Norms.Add(jointType, Z3Math.Real(Z3Math.GetRealValue(baseBody.Norms[jointType])));
            }
        }
コード例 #2
0
ファイル: Transforms.cs プロジェクト: matheusberger/prepose
        /// <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);
        }
コード例 #3
0
ファイル: Pose.cs プロジェクト: matheusberger/prepose
        public bool IsBodyAccepted(
            Z3Body input)
        {
            bool result = this.Restriction.IsBodyAccepted(input);

            return(result);
        }
コード例 #4
0
        public bool IsBodyAccepted(Z3Body body, Context localContext)
        {
            BoolExpr resultExpr = Evaluate(body);
            bool     result     = Z3.EvaluateBoolExpr(resultExpr, localContext);

            return(result);
        }
コード例 #5
0
ファイル: Safety.cs プロジェクト: matheusberger/prepose
        public static bool IsWithinDefaultSafetyRestrictions(Pose pose, out string firstBadStatement)
        {
            bool result = true;

            firstBadStatement = "";

            Z3Body   input                 = Z3Body.MkZ3Const();
            Z3Body   transformed           = pose.Transform.Transform(input);
            BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);

            var restrictions = Safety.DefaultSafetyRestriction().Restrictions;
            var composite    = new CompositeBodyRestriction();

            foreach (var restriction in restrictions)
            {
                composite.And(restriction);

                BoolExpr          inputSafe    = composite.Evaluate(transformed);
                BoolExpr          expr         = Z3.Context.MkAnd(transformedRestricted, inputSafe);
                SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);

                if (solverResult.Status == Status.UNSATISFIABLE)
                {
                    firstBadStatement = ((SimpleBodyRestriction)restriction).Message;
                    result            = false;
                    break;
                }
            }
            return(result);
        }
コード例 #6
0
        /// <summary>
        /// Produces a witness body given input restrictions, output
        /// restrictions and a transform.
        /// </summary>
        /// <param name="inputBodyRestriction"></param>
        /// <param name="bodyTransform"></param>
        /// <param name="outputBodyRestriction"></param>
        /// <returns></returns>
        public static Z3Body GenerateWitness(
            CompositeBodyRestriction inputBodyRestriction,
            BodyTransform bodyTransform,
            CompositeBodyRestriction outputBodyRestriction)
        {
            var body            = Z3Body.MkZ3Const();
            var transformedBody = bodyTransform.Transform(body);

            var expr1 = inputBodyRestriction.Evaluate(body);
            var expr2 = outputBodyRestriction.Evaluate(transformedBody);
            var expr  = Z3.Context.MkAnd(expr1, expr2);

            var evaluatedJoints =
                JointTypeHelper.MergeJointTypeLists(
                    inputBodyRestriction.GetJointTypes(),
                    bodyTransform.GetJointTypes(),
                    outputBodyRestriction.GetJointTypes());

            var checkResult = CheckStatus(expr);

            if (checkResult.Status == Status.SATISFIABLE)
            {
                var witness = CreateBodyWitness(
                    body,
                    checkResult.Model,
                    evaluatedJoints,
                    JointTypeHelper.CreateDefaultZ3Body());
                return(witness);
            }
            else
            {
                return(null);
            }
        }
コード例 #7
0
ファイル: Validity.cs プロジェクト: matheusberger/prepose
        public static bool IsInternallyValid(Pose pose, out string firstBadStatement)
        {
            bool result = true;

            firstBadStatement = "";
            Z3Body input        = Z3Body.MkZ3Const();
            Z3Body transformed  = pose.Transform.Transform(input);
            var    restrictions = pose.Restriction.Restrictions;

            var composite = new CompositeBodyRestriction();

            foreach (var restriction in restrictions)
            {
                composite.And(restriction);
                BoolExpr          transformedRestricted = composite.Evaluate(transformed);
                SolverCheckResult solverResult          = Z3AnalysisInterface.CheckStatus(transformedRestricted);

                if (solverResult.Status == Status.UNSATISFIABLE)
                {
                    firstBadStatement = restriction.ToString();
                    result            = false;
                    break;
                }
            }

            return(result);
        }
コード例 #8
0
ファイル: PoseBuilder.cs プロジェクト: matheusberger/prepose
        //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);
        }
コード例 #9
0
 private void Reset(Z3Body body)
 {
     this.CurrentStep = 0;
     //this.StepLastPercentage = 0;
     this.UpdateTargetBody(body);
     this.AccumulatedError = 0;
     this.Target           = null;
 }
コード例 #10
0
 public void InitBody(Z3Body body, Context localContext)
 {
     GestureMatcherLocalContext = localContext;
     if (this.Target == null)
     {
         this.UpdateTargetBody(body);
     }
 }
コード例 #11
0
 private void Reset(Z3Body body)
 {
     this.CurrentStep  = 0;
     this.LastDistance = 0;
     this.UpdateTargetBody(body);
     this.AccumulatedError = 0;
     this.Target           = null;
 }
コード例 #12
0
        public bool IsTransformAcceptedByRestriction()
        {
            Z3Body   body            = Z3Body.MkZ3Const();
            Z3Body   transformedBody = this.Transform.Transform(body);
            BoolExpr expr            = this.Restriction.Evaluate(transformedBody);

            SolverCheckResult checkResult = Z3AnalysisInterface.CheckStatus(expr);

            return(checkResult.Status != Status.UNSATISFIABLE);
        }
コード例 #13
0
ファイル: Pose.cs プロジェクト: matheusberger/prepose
        public bool IsTransformedBodyAccepted(Z3Body body)
        {
            bool result = false;

            Z3Body transformedBody = this.mTransform.Transform(body);

            result = this.IsBodyAccepted(transformedBody);

            return(result);
        }
コード例 #14
0
ファイル: Z3Body.cs プロジェクト: matheusberger/prepose
        public Dictionary <JointType, Z3Point3D> GrabDistancePoint3Ds(Z3Body that, List <JointType> joints)
        {
            var result = new Dictionary <JointType, Z3Point3D>();// Z3Math.Zero;

            foreach (var jointType in joints)
            {
                result.Add(jointType, this.Joints[jointType].GrabDistancePoint3D(that.Joints[jointType]));
            }

            return(result);
        }
コード例 #15
0
ファイル: Ambiguity.cs プロジェクト: matheusberger/prepose
        private static BoolExpr ExpressionFromCurrentStep(Gesture gesture1, int gesture1CurrentStep)
        {
            var input1 = Z3Body.MkZ3Const();
            var step1  = gesture1.Steps[gesture1CurrentStep];
            var pose1  = step1.Pose;

            input1 = pose1.Transform.Transform(input1);
            var expr1 = pose1.Restriction.Evaluate(input1);

            return(expr1);
        }
コード例 #16
0
ファイル: Gestures.cs プロジェクト: virrkharia/prepose
        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));
            }
        }
コード例 #17
0
ファイル: Z3Body.cs プロジェクト: matheusberger/prepose
        public BoolExpr IsNearerThan(Z3Body that, double distanceThreshold)
        {
            BoolExpr result = Z3.Context.MkTrue();

            var jointTypes = EnumUtil.GetValues <JointType>();

            foreach (var jointType in jointTypes)
            {
                result = Z3.Context.MkAnd(this.Joints[jointType].IsNearerThan(that.Joints[jointType], distanceThreshold));
            }

            return(result);
        }
コード例 #18
0
ファイル: Z3Body.cs プロジェクト: virrkharia/prepose
        public static Z3Body MkZ3Const()
        {
            Z3Body result = new Z3Body();

            var jointTypes = EnumUtil.GetValues <JointType>();

            foreach (var jointType in jointTypes)
            {
                result.Joints.Add(jointType, Z3Point3D.MkZ3Const(jointType.ToString()));
                result.Norms.Add(jointType, Z3.Context.MkRealConst(jointType.ToString() + " Norm"));
            }

            return(result);
        }
コード例 #19
0
ファイル: Z3Body.cs プロジェクト: virrkharia/prepose
        public Z3Body(Z3Body baseBody)
        {
            foreach (var jointType in baseBody.Joints.Keys)
            {
                this.Joints.Add(jointType,
                                new Z3Point3D(
                                    Z3Math.GetRealValue(baseBody.Joints[jointType].X),
                                    Z3Math.GetRealValue(baseBody.Joints[jointType].Y),
                                    Z3Math.GetRealValue(baseBody.Joints[jointType].Z)
                                    ));

                this.Norms.Add(jointType, Z3Math.Real(Z3Math.GetRealValue(baseBody.Norms[jointType])));
            }
        }
コード例 #20
0
        /// <summary>
        /// Generates a witness body tha matches the restrictions that
        /// are passed in.
        /// </summary>
        /// <param name="bodyRestriction"></param>
        /// <returns></returns>
        public static Z3Body GenerateWitness(IBodyRestriction bodyRestriction)
        {
            var body = Z3Body.MkZ3Const();

            var expr = bodyRestriction.Evaluate(body);

            var checkResult = CheckStatus(expr);
            var witness     = CreateBodyWitness(
                body,
                checkResult.Model,
                bodyRestriction.GetJointTypes(),
                JointTypeHelper.CreateDefaultZ3Body());

            return(witness);
        }
コード例 #21
0
        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);
        }
コード例 #22
0
        public BoolExpr Evaluate(Z3Body body)
        {
            Func <Z3Body, BoolExpr> composedExpr = bodyInput =>
            {
                BoolExpr result = Z3.Context.MkTrue();

                foreach (var restriction in this.Restrictions)
                {
                    result = Z3.Context.MkAnd(result, restriction.Evaluate(bodyInput));
                }

                return(result);
            };

            return(composedExpr(body));
        }
コード例 #23
0
ファイル: Z3Body.cs プロジェクト: matheusberger/prepose
        //public Dictionary<JointType, ArithExpr> CalcDistanceExprs(Z3Target target)
        //{
        //    return CalcDistanceExprs(target.Body, target.TransformedJoints);
        //}

        public BoolExpr IsDegreesBetweenLessThan(Z3Body that, List <JointType> joints, int degreesThreshold)
        {
            BoolExpr result = Z3Math.True;

            foreach (var jointType in joints)
            {
                // Only add distance if the target (that) body has the joint active
                // This allows partial matching, evaluating only the needed joints for the pose
                result =
                    Z3.Context.MkAnd(
                        result,
                        this.Joints[jointType].IsDegreesBetweenLessThan(that.Joints[jointType], degreesThreshold));
            }

            return(result);
        }
コード例 #24
0
ファイル: Z3Body.cs プロジェクト: matheusberger/prepose
        // Calculates average joint distance to target body considering only the active joints
        public Dictionary <JointType, ArithExpr> CalcDistanceExprs(Z3Body that, List <JointType> joints)
        {
            var result = new Dictionary <JointType, ArithExpr>();// Z3Math.Zero;

            foreach (var jointType in joints)
            {
                // Only add distance if the target (that) body has the joint active
                // This allows partial matching, evaluating only the needed joints for the pose
                //result = Z3Math.Add(result, this.Joints[jointType].CalcApproximateDistance(that.Joints[jointType]));
                //result = Z3Math.Max(result, this.Joints[jointType].CalcApproximateDistance(that.Joints[jointType]));
                result.Add(jointType, this.Joints[jointType].CalcApproximateDistance(that.Joints[jointType]));
            }
            //result = Z3Math.Div(result, Z3Math.Real(joints.Count));

            return(result);
        }
コード例 #25
0
        // 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);
            }
        }
コード例 #26
0
        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);
        }
コード例 #27
0
        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++;
        }
コード例 #28
0
        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);
        }
コード例 #29
0
ファイル: Z3Body.cs プロジェクト: matheusberger/prepose
        public BoolExpr IsEqualsTo(Z3Body that)
        {
            BoolExpr result    = Z3Math.True;
            var      allJoints = EnumUtil.GetValues <JointType>();

            foreach (var jointType in allJoints)
            {
                // Only add distance if the target (that) body has the joint active
                // This allows partial matching, evaluating only the needed joints for the pose
                result =
                    Z3.Context.MkAnd(
                        result,
                        this.Joints[jointType].IsEqualTo(that.Joints[jointType]));
            }

            return(result);
        }
コード例 #30
0
        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)));
                }
            }
        }