public override void UpdateValue() { if (matSourceGetterInterface == null) { return; } if (faceLandmarkGetterInterface == null) { return; } Mat rgbaMat = matSourceGetterInterface.GetMatSource(); if (rgbaMat == null) { return; } else { if (rgbaMat.width() != imageWidth || rgbaMat.height() != imageHeight) { imageWidth = rgbaMat.width(); imageHeight = rgbaMat.height(); SetCameraMatrix(camMatrix, imageWidth, imageHeight); } } didUpdateHeadPositionAndRotation = false; List <Vector2> points = faceLandmarkGetterInterface.GetFaceLanmarkPoints(); if (points != null) { MatOfPoint3f objectPoints = null; if (points.Count == 68) { objectPoints = objectPoints68; 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 (Tip) new Point(points[33].x, points[33].y), //nose (Subnasale) new Point(points[0].x, points[0].y), //l ear (Bitragion breadth) new Point(points[16].x, points[16].y) //r ear (Bitragion breadth) ); } else if (points.Count == 17) { objectPoints = objectPoints17; imagePoints.fromArray( new Point((points[2].x + points[3].x) / 2, (points[2].y + points[3].y) / 2), //l eye (Interpupillary breadth) new Point((points[4].x + points[5].x) / 2, (points[4].y + points[5].y) / 2), //r eye (Interpupillary breadth) new Point(points[0].x, points[0].y), //nose (Tip) new Point(points[1].x, points[1].y), //nose (Subnasale) new Point(points[6].x, points[6].y), //l ear (Bitragion breadth) new Point(points[8].x, points[8].y) //r ear (Bitragion breadth) ); } else if (points.Count == 6) { objectPoints = objectPoints6; imagePoints.fromArray( new Point((points[2].x + points[3].x) / 2, (points[2].y + points[3].y) / 2), //l eye (Interpupillary breadth) new Point((points[4].x + points[5].x) / 2, (points[4].y + points[5].y) / 2), //r eye (Interpupillary breadth) new Point(points[0].x, points[0].y), //nose (Tip) new Point(points[1].x, points[1].y) //nose (Subnasale) ); } // 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_x = tvec.get(0, 0)[0], tvec_y = tvec.get(1, 0)[0], tvec_z = tvec.get(2, 0)[0]; bool isNotInViewport = false; Vector4 pos = VP * new Vector4((float)tvec_x, (float)tvec_y, (float)tvec_z, 1.0f); if (pos.w != 0) { float x = pos.x / pos.w, y = pos.y / pos.w, z = pos.z / pos.w; if (x < -1.0f || x > 1.0f || y < -1.0f || y > 1.0f || z < -1.0f || z > 1.0f) { isNotInViewport = true; } } if (double.IsNaN(tvec_z) || isNotInViewport) { // if tvec is wrong data, do not use extrinsic guesses. (the estimated object is not in the camera field of view) Calib3d.solvePnP(objectPoints, imagePoints, camMatrix, distCoeffs, rvec, tvec); } else { Calib3d.solvePnP(objectPoints, imagePoints, camMatrix, distCoeffs, rvec, tvec, true, Calib3d.SOLVEPNP_ITERATIVE); } //Debug.Log (tvec.dump () + " " + isNotInViewport); if (!isNotInViewport) { // 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); PoseData poseData = ARUtils.ConvertRvecTvecToPoseData(rvecArr, tvecArr); // adjust the position to the scale of real-world space. poseData.pos = new Vector3(poseData.pos.x * 0.001f, poseData.pos.y * 0.001f, poseData.pos.z * 0.001f); // Changes in pos/rot below these thresholds are ignored. if (enableLowPassFilter) { ARUtils.LowpassPoseData(ref oldPoseData, ref poseData, positionLowPass, rotationLowPass); } oldPoseData = poseData; Matrix4x4 transformationM = Matrix4x4.TRS(poseData.pos, poseData.rot, Vector3.one); // right-handed coordinates system (OpenCV) to left-handed one (Unity) // https://stackoverflow.com/questions/30234945/change-handedness-of-a-row-major-4x4-transformation-matrix transformationM = invertYM * transformationM * invertYM; // Apply Y-axis and Z-axis refletion matrix. (Adjust the posture of the AR object) transformationM = transformationM * invertYM * invertZM; headPosition = ARUtils.ExtractTranslationFromMatrix(ref transformationM); headRotation = ARUtils.ExtractRotationFromMatrix(ref transformationM); didUpdateHeadPositionAndRotation = true; } } }
//NOTE: ここList渡しなのがちょっとイヤなんだけど、Dlibのマナーに沿うことを重視 /// <summary> /// 特徴点を受け取って頭部姿勢の推定状況を更新します。 /// </summary> /// <param name="landmarks"></param> public void EstimatePose(List <Vector2> landmarks) { //画像の初期化が終わってない場合は完全NG if (_width < 1 || _height < 1) { return; } HasValidPoseData = false; SetImagePoints(landmarks); //初期推定っぽいやつ if (_rVec == null || _tVec == null) { _rVec = new Mat(3, 1, CvType.CV_64FC1); _tVec = new Mat(3, 1, CvType.CV_64FC1); Calib3d.solvePnP(_objPoints, _imagePoints, _camMatrix, _distCoeffs, _rVec, _tVec); } _tVec.get(0, 0, _tVecArrayTemp); float tx = (float)_tVecArrayTemp[0]; float ty = (float)_tVecArrayTemp[1]; float tz = (float)_tVecArrayTemp[2]; bool notInViewport = false; var pos = _vp * new Vector4(tx, ty, tz, 1.0f); if (Mathf.Abs(pos.w) > 0.0001) { float x = pos.x / pos.w; float y = pos.y / pos.w; float z = pos.z / pos.w; notInViewport = Mathf.Abs(x) > 1.0f || Mathf.Abs(y) > 1.0f || Mathf.Abs(z) > 1.0f; } //NOTE: 要するに現状の推定が怪しすぎるならゼロベースで計算をやり直せ、って話 if (float.IsNaN(tz) || notInViewport) { Calib3d.solvePnP(_objPoints, _imagePoints, _camMatrix, _distCoeffs, _rVec, _tVec); } else { //普通にトラッキングできてればこっちのはず Calib3d.solvePnP( _objPoints, _imagePoints, _camMatrix, _distCoeffs, _rVec, _tVec, true, Calib3d.SOLVEPNP_ITERATIVE ); } if (notInViewport) { return; } // 最終的なt/rの値をUnityで使いたいのでPosition/Rotationに変換 _rVec.get(0, 0, _rVecArray); _tVec.get(0, 0, _tVecArray); var poseData = ARUtils.ConvertRvecTvecToPoseData(_rVecArray, _tVecArray); // 0.001fは[mm]単位から[m]への変換 // YMのほう: 右手系を左手系にする // ZMのほう: 手前/奥をひっくり返す(はず) var transformationM = _invertYM * Matrix4x4.TRS(0.001f * poseData.pos, poseData.rot, Vector3.one) * _invertZM; HeadPosition = ARUtils.ExtractTranslationFromMatrix(ref transformationM); HeadRotation = ARUtils.ExtractRotationFromMatrix(ref transformationM); HasValidPoseData = true; }