private void UpdateARObjectTransform_2(Mat rvec, Mat tvec) { // 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); // Changes in pos/rot below these thresholds are ignored. if (enableLowPassFilter) { ARUtils.LowpassPoseData(ref oldPoseData, ref poseData, positionLowPass, rotationLowPass); } oldPoseData = poseData; // Convert to transform matrix. ARM = ARUtils.ConvertPoseDataToMatrix(ref poseData, true, true); //if (shouldMoveARCamera) { // ARM = arGameObject.transform.localToWorldMatrix * ARM.inverse; // ARUtils.SetTransformFromMatrix(arCamera.transform, ref ARM); //} else { ARM = arCamera.transform.localToWorldMatrix * ARM; ARUtils.SetTransformFromMatrix(arGameObject.transform, ref ARM); //} }
private void UpdateARObjectTransform(Mat rvec, Mat tvec, int id) { // Find the object of this id if (idDict.ContainsKey(id)) { arGameObject = GameObject.Find(idDict[id]); } // 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); // Changes in pos/rot below these thresholds are ignored. if (enableLowPassFilter) { ARUtils.LowpassPoseData(ref oldPoseData, ref poseData, positionLowPass, rotationLowPass); } oldPoseData = poseData; // Convert to transform matrix. ARM = ARUtils.ConvertPoseDataToMatrix(ref poseData, true, true); ARM = arCamera.transform.localToWorldMatrix * ARM; ARUtils.SetTransformFromMatrix(arGameObject.transform, ref ARM); //Only move and do not rotate Vector3 rawRotation = arGameObject.transform.rotation.eulerAngles; arGameObject.transform.rotation = Quaternion.Euler(new Vector3(0, adjustRotation(rawRotation[1]), 0)); }
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; } } }
// Update is called once per frame public override void UpdateValue() { if (dlibFaceLanmarkGetter == null) { return; } didUpdateHeadRotation = false; List <Vector2> points = dlibFaceLanmarkGetter.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 (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); } //Debug.Log (tvec.dump()); if (!double.IsNaN(tvec_z)) { // 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); // Changes in pos/rot below these thresholds are ignored. if (enableLowPassFilter) { ARUtils.LowpassPoseData(ref oldPoseData, ref poseData, positionLowPass, rotationLowPass); } oldPoseData = poseData; 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; headRotation = ARUtils.ExtractRotationFromMatrix(ref ARM); didUpdateHeadRotation = true; } } }
/// <summary> /// 顔の向きを取得 /// </summary> private Vector3 GetFrontalFaceAngle(List <Vector2> points) { if (points.Count != 68) { throw new ArgumentNullException("ランドマークが正しくありません。"); } if (_camMatrix == null) { throw new ArgumentNullException("カメラの内部パラメータが正しくありません。"); } // スケールの正規化 float normScale = Math.Abs(points[30].y - points[8].y) / (normHeight / 2); Vector2 normDiff = points[30] * normScale - new Vector2(normWidth / 2, normHeight / 2); for (int i = 0; i < points.Count; i++) { _normPoints[i] = points[i] * normScale - normDiff; } _imagePoints.fromArray( new Point((_normPoints[38].x + _normPoints[41].x) / 2, (_normPoints[38].y + _normPoints[41].y) / 2), // 左目 new Point((_normPoints[43].x + _normPoints[46].x) / 2, (_normPoints[43].y + _normPoints[46].y) / 2), // 右目 new Point(_normPoints[33].x, _normPoints[33].y), // 鼻 new Point(_normPoints[48].x, _normPoints[48].y), // 左口角 new Point(_normPoints[54].x, _normPoints[54].y), // 右口角 new Point(_normPoints[0].x, _normPoints[0].y), // 左耳 new Point(_normPoints[16].x, _normPoints[16].y) // 右耳 ); // 2-3次元対応点から頭部を姿勢推定する 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 tVecX = _tVec.get(0, 0)[0]; double tVecY = _tVec.get(1, 0)[0]; double tVecZ = _tVec.get(2, 0)[0]; bool isNotInViewport = false; Vector4 pos = _VP * new Vector4((float)tVecX, (float)tVecY, (float)tVecZ, 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(tVecZ) || isNotInViewport) { Calib3d.solvePnP(_objectPoints, _imagePoints, _camMatrix, _distCoeffs, _rVec, _tVec); } else { Calib3d.solvePnP(_objectPoints, _imagePoints, _camMatrix, _distCoeffs, _rVec, _tVec, true, Calib3d.SOLVEPNP_ITERATIVE); } if (!isNotInViewport) { // Unityのポーズデータに変換 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); // 閾値以下ならば更新を無視 ARUtils.LowpassPoseData(ref _oldPoseData, ref poseData, positionLowPass, rotationLowPass); _oldPoseData = poseData; // 変換行列を作成 _transformationM = Matrix4x4.TRS(poseData.pos, poseData.rot, Vector3.one); } Calib3d.Rodrigues(_rVec, _rotM); _transformationM.SetRow(0, new Vector4( (float)_rotM.get(0, 0)[0], (float)_rotM.get(0, 1)[0], (float)_rotM.get(0, 2)[0], (float)_tVec.get(0, 0)[0])); _transformationM.SetRow(1, new Vector4( (float)_rotM.get(1, 0)[0], (float)_rotM.get(1, 1)[0], (float)_rotM.get(1, 2)[0], (float)_tVec.get(1, 0)[0])); _transformationM.SetRow(2, new Vector4( (float)_rotM.get(2, 0)[0], (float)_rotM.get(2, 1)[0], (float)_rotM.get(2, 2)[0], (float)_tVec.get(2, 0)[0])); _transformationM.SetRow(3, new Vector4( 0, 0, 0, 1)); _ARM = _invertYM * _transformationM * _invertYM; _ARM = _ARM * _invertYM * _invertZM; return(ExtractRotationFromMatrix(ref _ARM).eulerAngles); }