private double CalculateYAngle(KeyPoint2D keyPointOne, KeyPoint2D keyPointTwo, double length) { return(CalculateYAngle(keyPointOne.Y, keyPointTwo.Y, length)); }
public Pose3D Simulate3D() { //KeyPoint2D centerPoint = GetKeyPoint2D(BodyPoint.Center_Body); List <KeyPoint3D> temp = new List <KeyPoint3D>(); foreach (KeyPoint2D keypoint in KeyPoints) { // Left hand Z axis and rotation estimation if (keypoint.IsValid && keypoint.BodyPoint == BodyPoint.Left_Wrist) { KeyPoint2D leftElbow = GetKeyPoint2D(BodyPoint.Left_Elbow); KeyPoint2D leftShoulder = GetKeyPoint2D(BodyPoint.Left_Shoulder); //double distScale = 1; double upperArmZ = 0; double lowerArmZ = 0; double leftHandYaw = 0; double leftHandPitch = 0; double leftHandRoll = 0; if (leftElbow != null && leftElbow.IsValid) { if (leftShoulder != null && leftShoulder.IsValid) { upperArmZ += CalculateZ(leftShoulder, leftElbow, Simulated3DSettings.UpperLeftArmLength); } lowerArmZ += CalculateZ(keypoint, leftElbow, Simulated3DSettings.LowerLeftArmLength); // - 90 degrees to aim forward for both of these leftHandYaw += CalculateXAngle(keypoint, leftElbow, Simulated3DSettings.LowerLeftArmLength) - 90; leftHandPitch += CalculateYAngle(leftElbow, keypoint, Simulated3DSettings.LowerLeftArmLength) - 90; } temp.Add(new KeyPoint3D(keypoint.BodyPoint, keypoint.Raw_X, keypoint.Raw_Y, upperArmZ + lowerArmZ, keypoint.Score, leftHandYaw, leftHandPitch, leftHandRoll)); } // Right hand Z axis and rotation estimation else if (keypoint.IsValid && keypoint.BodyPoint == BodyPoint.Right_Wrist) { KeyPoint2D rightElbow = GetKeyPoint2D(BodyPoint.Right_Elbow); KeyPoint2D rightShoulder = GetKeyPoint2D(BodyPoint.Right_Shoulder); //double distScale = 1; double upperArmZ = 0; double lowerArmZ = 0; double rightHandYaw = 0; double rightHandPitch = 0; double rightHandRoll = 0; if (rightElbow != null && rightElbow.IsValid) { if (rightShoulder != null && rightShoulder.IsValid) { upperArmZ += CalculateZ(rightShoulder, rightElbow, Simulated3DSettings.UpperRightArmLength); //distScale += armDisp; } lowerArmZ += CalculateZ(keypoint, rightElbow, Simulated3DSettings.LowerRightArmLength); // - 90 degrees to aim forward for both of these rightHandYaw += CalculateXAngle(keypoint, rightElbow, Simulated3DSettings.LowerLeftArmLength) - 90; rightHandPitch += CalculateYAngle(rightElbow, keypoint, Simulated3DSettings.LowerLeftArmLength) - 90; } temp.Add(new KeyPoint3D(keypoint.BodyPoint, keypoint.Raw_X, keypoint.Raw_Y, upperArmZ + lowerArmZ, keypoint.Score, rightHandYaw, rightHandPitch, rightHandRoll)); } // Head Z axis and rotation estimation else if (keypoint.IsValid && keypoint.BodyPoint == BodyPoint.Nose_or_Top_Head) { double headZ = 0; double headYaw = 0; double headPitch = 0; double headRoll = 0; KeyPoint2D neck = GetKeyPoint2D(BodyPoint.Bottom_Neck); if (neck != null && neck.IsValid) { headZ = CalculateZ(keypoint, neck, (OpenPose_Reader.Model == Model.COCO ? Simulated3DSettings.NeckLength_COCO : Simulated3DSettings.NeckLength_MPI)); headPitch = -CalculateZAngle(keypoint, neck, (OpenPose_Reader.Model == Model.COCO ? Simulated3DSettings.NeckLength_COCO : Simulated3DSettings.NeckLength_MPI)); } if (OpenPose_Reader.Model == Model.COCO) { KeyPoint2D leftEye = GetKeyPoint2D(BodyPoint.Left_Eye); KeyPoint2D rightEye = GetKeyPoint2D(BodyPoint.Right_Eye_or_Center_Body); if (leftEye != null && leftEye.IsValid && rightEye != null && rightEye.IsValid) { headYaw = CalculateZAngle(leftEye, rightEye, (Simulated3DSettings.EyeDist)); KeyPoint2D leftEar = GetKeyPoint2D(BodyPoint.Left_Ear); KeyPoint2D rightEar = GetKeyPoint2D(BodyPoint.Right_Ear); headYaw = (leftEar != null && leftEar.IsValid) || (rightEar == null || !rightEar.IsValid) ? headYaw : -headYaw; } } temp.Add(new KeyPoint3D(keypoint.BodyPoint, keypoint.Raw_X, keypoint.Raw_Y, headZ, keypoint.Score, headYaw, headPitch, headRoll)); } else { temp.Add(new KeyPoint3D(keypoint.BodyPoint, keypoint.Raw_X, keypoint.Raw_Y, 0, keypoint.Score)); } } return(new Pose3D(temp.ToArray())); }
private double CalculateBaseAngle(KeyPoint2D keyPointOne, KeyPoint2D keyPointTwo) { return(CalculateBaseAngle(keyPointOne.X, keyPointTwo.X, keyPointOne.Y, keyPointTwo.Y)); }