private void UpdateARHeadTransform(List <Vector2> points)
{
// The coordinates in pixels of the object detected on the image projected onto the plane.
imagePoints.fromArray(
new Point((points [38].x + points [41].x) / 2, (points [38].y + points [41].y) / 2), //l eye (Interpupillary breadth)
new Point((points [43].x + points [46].x) / 2, (points [43].y + points [46].y) / 2), //r eye (Interpupillary breadth)
new Point(points [30].x, points [30].y), //nose (Nose top)
new Point(points [48].x, points [48].y), //l mouth (Mouth breadth)
new Point(points [54].x, points [54].y), //r mouth (Mouth breadth)
new Point(points [0].x, points [0].y), //l ear (Bitragion breadth)
new Point(points [16].x, points [16].y) //r ear (Bitragion breadth)
);
// Estimate head pose.
if (rvec == null || tvec == null)
{
rvec = new Mat(3, 1, CvType.CV_64FC1);
tvec = new Mat(3, 1, CvType.CV_64FC1);
Calib3d.solvePnP(objectPoints, imagePoints, camMatrix, distCoeffs, rvec, tvec);
}
double tvec_z = tvec.get(2, 0) [0];
if (double.IsNaN(tvec_z) || tvec_z < 0) // if tvec is wrong data, do not use extrinsic guesses.
{
Calib3d.solvePnP(objectPoints, imagePoints, camMatrix, distCoeffs, rvec, tvec);
}
else
{
Calib3d.solvePnP(objectPoints, imagePoints, camMatrix, distCoeffs, rvec, tvec, true, Calib3d.SOLVEPNP_ITERATIVE);
}
if (applyEstimationPose && !double.IsNaN(tvec_z))
{
if (Mathf.Abs((float)(points [43].y - points [46].y)) > Mathf.Abs((float)(points [42].x - points [45].x)) / 6.0)
{
if (displayEffects)
{
rightEye.SetActive(true);
}
}
if (Mathf.Abs((float)(points [38].y - points [41].y)) > Mathf.Abs((float)(points [39].x - points [36].x)) / 6.0)
{
if (displayEffects)
{
leftEye.SetActive(true);
}
}
if (displayHead)
{
head.SetActive(true);
}
if (displayAxes)
{
axes.SetActive(true);
}
float noseDistance = Mathf.Abs((float)(points [27].y - points [33].y));
float mouseDistance = Mathf.Abs((float)(points [62].y - points [66].y));
if (mouseDistance > noseDistance / 5.0)
{
if (displayEffects)
{
mouth.SetActive(true);
foreach (ParticleSystem ps in mouthParticleSystem)
{
var em = ps.emission;
em.enabled = true;
#if UNITY_5_5_OR_NEWER
var main = ps.main;
main.startSizeMultiplier = 40 * (mouseDistance / noseDistance);
#else
ps.startSize = 40 * (mouseDistance / noseDistance);
#endif
}
}
}
else
{
if (displayEffects)
{
foreach (ParticleSystem ps in mouthParticleSystem)
{
var em = ps.emission;
em.enabled = false;
}
}
}
Calib3d.Rodrigues(rvec, rotMat);
transformationM.SetRow(0, new Vector4((float)rotMat.get(0, 0) [0], (float)rotMat.get(0, 1) [0], (float)rotMat.get(0, 2) [0], (float)(tvec.get(0, 0) [0] / 1000.0)));
transformationM.SetRow(1, new Vector4((float)rotMat.get(1, 0) [0], (float)rotMat.get(1, 1) [0], (float)rotMat.get(1, 2) [0], (float)(tvec.get(1, 0) [0] / 1000.0)));
transformationM.SetRow(2, new Vector4((float)rotMat.get(2, 0) [0], (float)rotMat.get(2, 1) [0], (float)rotMat.get(2, 2) [0], (float)(tvec.get(2, 0) [0] / 1000.0 / imageOptimizationHelper.downscaleRatio)));
transformationM.SetRow(3, new Vector4(0, 0, 0, 1));
// right-handed coordinates system (OpenCV) to left-handed one (Unity)
ARM = invertYM * transformationM;
// Apply Z axis inverted matrix.
ARM = ARM * invertZM;
// Apply camera transform matrix.
ARM = arCamera.transform.localToWorldMatrix * ARM;
ARUtils.SetTransformFromMatrix(arGameObject.transform, ref ARM);
}
}
