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 [33].x, points [33].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. Calib3d.solvePnP(objectPoints, imagePoints, camMatrix, distCoeffs, rvec, tvec); if (tvec.get(2, 0) [0] > 0) { if (Mathf.Abs((float)(points [43].y - points [46].y)) > Mathf.Abs((float)(points [42].x - points [45].x)) / 6.0) { if (isShowingEffects) { 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 (isShowingEffects) { leftEye.SetActive(true); } } if (isShowingHead) { head.SetActive(true); } if (isShowingAxes) { 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 (isShowingEffects) { mouth.SetActive(true); foreach (ParticleSystem ps in mouthParticleSystem) { ps.enableEmission = true; ps.startSize = 40 * (mouseDistance / noseDistance); } } } else { if (isShowingEffects) { foreach (ParticleSystem ps in mouthParticleSystem) { ps.enableEmission = 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)); 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)); 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 / webCamTextureToMatHelper.DOWNSCALE_RATIO)); 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); } }
private void UpdateARHeadTransform(List <Vector2> points, Matrix4x4 cameraToWorldMatrix) { // 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)) / 5.0) { if (displayEffects) { rightEye.SetActive(true); } } else { if (displayEffects) { rightEye.SetActive(false); } } if (Mathf.Abs((float)(points [38].y - points [41].y)) > Mathf.Abs((float)(points [39].x - points [36].x)) / 5.0) { if (displayEffects) { leftEye.SetActive(true); } } else { if (displayEffects) { leftEye.SetActive(false); } } 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; } } } // Convert to unity pose data. double[] rvecArr = new double[3]; rvec.get(0, 0, rvecArr); double[] tvecArr = new double[3]; tvec.get(0, 0, tvecArr); tvecArr [0] = tvecArr [0] / 1000.0; tvecArr[1] = tvecArr[1] / 1000.0; tvecArr[2] = tvecArr[2] / 1000.0 / imageOptimizationHelper.downscaleRatio; PoseData poseData = ARUtils.ConvertRvecTvecToPoseData(rvecArr, tvecArr); // Changes in pos/rot below these thresholds are ignored. if (enableLowPassFilter) { ARUtils.LowpassPoseData(ref oldPoseData, ref poseData, positionLowPass, rotationLowPass); } oldPoseData = poseData; // Create transform matrix. transformationM = Matrix4x4.TRS(poseData.pos, poseData.rot, Vector3.one); // right-handed coordinates system (OpenCV) to left-handed one (Unity) ARM = invertYM * transformationM; // Apply Z-axis inverted matrix. ARM = ARM * invertZM; // Apply the cameraToWorld matrix with the Z-axis inverted. ARM = cameraToWorldMatrix * invertZM * ARM; ARUtils.SetTransformFromMatrix(arGameObject.transform, ref ARM); } }
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); } }