public List <Vector3> GetConfidentPositions(UJoint[] joints) { List <Vector3> positions = new List <Vector3>(); for (int i = 0; i < joints.Length; i++) { UJoint j = joints[i]; if (j.Confidence == JointConfidenceLevel.Low || j.Confidence == JointConfidenceLevel.None) { continue; } positions.Add(j.Position); } return(positions); }
/// <summary> /// Get all joints with their original position and rotation /// </summary> /// <returns></returns> public UJoint[] ResolveSkeleton() { //Debug.Log("Resolving Skeleton"); //get all joint positions UJoint[] joints = new UJoint[27]; for (int i = 0; i < 27; i++) { Joint j = trackedBody.GetJoint(i); UJoint unityJoint = new UJoint(j); joints[i] = unityJoint; } if (AppManager.recording) { MotionManager.Instance.StoreFrame(joints); } return(joints); }
/// <summary> /// Determine how much alike two poses are to one another. Return the result in percent /// </summary> /// <param name="lhs"></param> /// <param name="rhs"></param> /// <returns></returns> public int ComparePoses(UJoint[] lhs, UJoint[] rhs) { if (lhs == null || rhs == null) { return(-1); } //JointId[] jointConstraints = AppManager.jointConstraints; Vector3 diffSum = Vector3.zero; Vector3 diffMirrorSum = Vector3.zero; Vector3 originalPelvisPosition = lhs[(int)Pelvis].Position; Vector3 cPelvisPos = rhs[(int)Pelvis].Position; Vector3 mirrorComparePelvisPosition = new Vector3(cPelvisPos.x * -1f, cPelvisPos.y, cPelvisPos.z); //handle unequal length arrays if (lhs.Length != rhs.Length) { Debug.Log("poses had different amount of joints"); return(-1); } int comparedJointsCount = 0; for (int i = 1; i < lhs.Length; i++) { UJoint pI = lhs[i]; //don't compare obstructed or out of range joints if (pI.Confidence == JointConfidenceLevel.Low || pI.Confidence == JointConfidenceLevel.None) { continue; } /* * bool jointConstraint = false; * foreach(var j in jointConstraints) * { * if (i == (int)j) * { * jointConstraint = true; * break; * } * } * if (jointConstraint) * continue; */ //joint positions Vector3 oPos = lhs[i].Position; Vector3 cPos = rhs[i].Position; Vector3 cPosMirror = new Vector3(cPos.x * -1, cPos.y, cPos.z); //joint positions relative to pelvis Vector3 relOPos = oPos - originalPelvisPosition; Vector3 relCPos = cPos - cPelvisPos; Vector3 relCPosMirror = cPosMirror - mirrorComparePelvisPosition; Vector3 diff = relCPos - relOPos; Vector3 diffMirror = relCPosMirror - relOPos; diffSum += diff.Square(); diffMirrorSum += diffMirror.Square(); comparedJointsCount++; } if (comparedJointsCount == 0) { Debug.LogWarning("All joints had JointConfidenceLevel.Low or .None"); } //average positional difference in mm Vector3 diffAverage = diffSum /= comparedJointsCount; Vector3 diffMirrorAverage = diffMirrorSum /= comparedJointsCount; float diffDistance = Mathf.Sqrt(diffAverage.x * diffAverage.x + diffAverage.y * diffAverage.y + diffAverage.z * diffAverage.z); float diffMirrorDistance = Mathf.Sqrt(diffMirrorAverage.x * diffMirrorAverage.x + diffMirrorAverage.y * diffMirrorAverage.y + diffMirrorAverage.z * diffMirrorAverage.z); if (diffDistance == float.NaN || diffMirrorDistance == float.NaN) { return(0); } //Debug.Log("DiffDistance: " + diffDistance + ", DiffDistanceMirrored: " + diffMirrorDistance); if (diffMirrorDistance < diffDistance) { diffDistance = diffMirrorDistance; } int percent; if (diffDistance > ZERO_PERCENT) { percent = 0; } else if (diffDistance < HUNDRED_PERCENT) { percent = 100; } else { percent = 100 - Mathf.RoundToInt((diffDistance - HUNDRED_PERCENT) / (ZERO_PERCENT * 0.01f)); } //Debug.Log("Positional difference in mm: " + posDifferenceD); return(percent); }