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));
}
