using System; using System.Collections.Generic; using UnityEngine; namespace OpenCVForUnity.UnityUtils { public struct PoseData { public Vector3 pos; public Quaternion rot; } /// /// AR utilities. /// public class ARUtils { /// /// Convertes rvec value to rotation transform. /// /// Rvec. /// Rotation. public static Quaternion ConvertRvecToRot(IList rvec) { if (rvec == null) throw new ArgumentNullException("rvec"); if (rvec.Count < 3) throw new ArgumentException("rvec.Count < 3"); Vector3 _rvec = new Vector3((float)rvec[0], (float)rvec[1], (float)rvec[2]); float theta = _rvec.magnitude; _rvec.Normalize(); // http://stackoverflow.com/questions/12933284/rodrigues-into-eulerangles-and-vice-versa return Quaternion.AngleAxis(theta * Mathf.Rad2Deg, _rvec); } /// /// Convertes tvec value to position transform. /// /// Tvec. /// Position. public static Vector3 ConvertTvecToPos(IList tvec) { if (tvec == null) throw new ArgumentNullException("tvec"); if (tvec.Count < 3) throw new ArgumentException("tvec.Count < 3"); if (tvec.Count < 3) return new Vector3(); return new Vector3((float)tvec[0], (float)tvec[1], (float)tvec[2]); } /// /// Convertes rvec and tvec value to PoseData. /// /// Rvec. /// Tvec. /// PoseData. public static PoseData ConvertRvecTvecToPoseData(IList rvec, IList tvec) { PoseData data = new PoseData(); data.pos = ConvertTvecToPos(tvec); data.rot = ConvertRvecToRot(rvec); return data; } private static Vector3 vector_one = Vector3.one; private static Matrix4x4 invertYMatrix = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, new Vector3(1, -1, 1)); /// /// Convertes PoseData to transform matrix. /// /// PoseData. /// Determines if convert the transformation matrix to the left-hand coordinate system. /// Transform matrix. public static Matrix4x4 ConvertPoseDataToMatrix(ref PoseData poseData, bool toLeftHandCoordinateSystem = false) { Matrix4x4 matrix = Matrix4x4.TRS(poseData.pos, poseData.rot, vector_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 if (toLeftHandCoordinateSystem) matrix = invertYMatrix * matrix * invertYMatrix; return matrix; } /// /// Convertes transform matrix to PoseData. /// /// Transform matrix. /// PoseData. public static PoseData ConvertMatrixToPoseData(ref Matrix4x4 matrix) { PoseData data = new PoseData(); data.pos = ExtractTranslationFromMatrix(ref matrix); data.rot = ExtractRotationFromMatrix(ref matrix); return data; } /// /// Creates pose data dictionary. /// /// ids. /// Rvecs. /// Tvecs. /// PoseData dictionary. public static Dictionary CreatePoseDataDict(IList ids, IList rvecs, IList tvecs) { if (ids == null) throw new ArgumentNullException("ids"); if (rvecs == null) throw new ArgumentNullException("rvecs"); if (tvecs == null) throw new ArgumentNullException("tvecs"); Dictionary dict = new Dictionary(); if (ids.Count == 0 || ids.Count * 3 != rvecs.Count || ids.Count * 3 != tvecs.Count) return dict; Vector3 rvec = new Vector3(); for (int i = 0; i < ids.Count; i++) { PoseData data = new PoseData(); data.pos.Set((float)tvecs[i * 3], (float)tvecs[i * 3 + 1], (float)tvecs[i * 3 + 2]); rvec.Set((float)rvecs[i * 3], (float)rvecs[i * 3 + 1], (float)rvecs[i * 3 + 2]); float theta = rvec.magnitude; rvec.Normalize(); data.rot = Quaternion.AngleAxis(theta * Mathf.Rad2Deg, rvec); dict[ids[i]] = data; } return dict; } /// /// Performs a lowpass check on the position and rotation in newPose, comparing them to oldPose. /// /// Old PoseData. /// New PoseData. /// Positon threshold. /// Rotation threshold. public static void LowpassPoseData(ref PoseData oldPose, ref PoseData newPose, float posThreshold, float rotThreshold) { posThreshold *= posThreshold; float posDiff = (newPose.pos - oldPose.pos).sqrMagnitude; float rotDiff = Quaternion.Angle(newPose.rot, oldPose.rot); if (posDiff < posThreshold) { newPose.pos = oldPose.pos; } if (rotDiff < rotThreshold) { newPose.rot = oldPose.rot; } } /// /// Performs a lowpass check on the position and rotation of each marker in newDict, comparing them to those in oldDict. /// /// Old dictionary. /// New dictionary. /// Positon threshold. /// Rotation threshold. public static void LowpassPoseDataDict(Dictionary oldDict, Dictionary newDict, float posThreshold, float rotThreshold) { if (oldDict == null) throw new ArgumentNullException("oldDict"); if (newDict == null) throw new ArgumentNullException("newDict"); posThreshold *= posThreshold; List keys = new List(newDict.Keys); foreach (int key in keys) { if (!oldDict.ContainsKey(key)) continue; PoseData oldPose = oldDict[key]; PoseData newPose = newDict[key]; float posDiff = (newPose.pos - oldPose.pos).sqrMagnitude; float rotDiff = Quaternion.Angle(newPose.rot, oldPose.rot); if (posDiff < posThreshold) { newPose.pos = oldPose.pos; } if (rotDiff < rotThreshold) { newPose.rot = oldPose.rot; } newDict[key] = newPose; } } /// /// Extract translation from transform matrix. /// /// Transform matrix. This parameter is passed by reference /// to improve performance; no changes will be made to it. /// /// Translation offset. /// public static Vector3 ExtractTranslationFromMatrix(ref Matrix4x4 matrix) { Vector3 translate; translate.x = matrix.m03; translate.y = matrix.m13; translate.z = matrix.m23; return translate; } /// /// Extract rotation quaternion from transform matrix. /// /// Transform matrix. This parameter is passed by reference /// to improve performance; no changes will be made to it. /// /// Quaternion representation of rotation transform. /// public static Quaternion ExtractRotationFromMatrix(ref Matrix4x4 matrix) { Vector3 forward; forward.x = matrix.m02; forward.y = matrix.m12; forward.z = matrix.m22; Vector3 upwards; upwards.x = matrix.m01; upwards.y = matrix.m11; upwards.z = matrix.m21; return Quaternion.LookRotation(forward, upwards); } /// /// Extract scale from transform matrix. /// /// Transform matrix. This parameter is passed by reference /// to improve performance; no changes will be made to it. /// /// Scale vector. /// public static Vector3 ExtractScaleFromMatrix(ref Matrix4x4 matrix) { Vector3 scale; scale.x = new Vector4(matrix.m00, matrix.m10, matrix.m20, matrix.m30).magnitude; scale.y = new Vector4(matrix.m01, matrix.m11, matrix.m21, matrix.m31).magnitude; scale.z = new Vector4(matrix.m02, matrix.m12, matrix.m22, matrix.m32).magnitude; if (Vector3.Cross(matrix.GetColumn(0), matrix.GetColumn(1)).normalized != (Vector3)matrix.GetColumn(2).normalized) { scale.x *= -1; } return scale; } /// /// Compose TRS matrix from position, rotation and scale. /// /// Position. /// Rotation. /// Scale. /// Transform matrix. public static void ComposeMatrix(Vector3 localPosition, Quaternion localRotation, Vector3 localScale, out Matrix4x4 matrix) { matrix = Matrix4x4.TRS(localPosition, localRotation, localScale); } /// /// Extract position, rotation and scale from TRS matrix. /// /// Transform matrix. This parameter is passed by reference /// to improve performance; no changes will be made to it. /// Position. /// Rotation. /// Scale. public static void DecomposeMatrix(ref Matrix4x4 matrix, out Vector3 localPosition, out Quaternion localRotation, out Vector3 localScale) { localPosition = ExtractTranslationFromMatrix(ref matrix); localRotation = ExtractRotationFromMatrix(ref matrix); localScale = ExtractScaleFromMatrix(ref matrix); } /// /// Set transform component from TRS matrix. /// /// Transform component. /// Transform matrix. This parameter is passed by reference /// to improve performance; no changes will be made to it. public static void SetTransformFromMatrix(Transform transform, ref Matrix4x4 matrix) { transform.localPosition = ExtractTranslationFromMatrix(ref matrix); transform.localRotation = ExtractRotationFromMatrix(ref matrix); transform.localScale = ExtractScaleFromMatrix(ref matrix); } /// /// Calculate projection matrix from camera matrix values. (OpenCV coordinate system to OpenGL coordinate system) /// /// Focal length x. /// Focal length y. /// Image center point x.(principal point x) /// Image center point y.(principal point y) /// Image width. /// Image height. /// The near clipping plane distance. /// The far clipping plane distance. /// /// Projection matrix. /// public static Matrix4x4 CalculateProjectionMatrixFromCameraMatrixValues(float fx, float fy, float cx, float cy, float width, float height, float near, float far) { Matrix4x4 projectionMatrix = new Matrix4x4(); projectionMatrix.m00 = 2.0f * fx / width; projectionMatrix.m02 = 1.0f - 2.0f * cx / width; projectionMatrix.m11 = 2.0f * fy / height; projectionMatrix.m12 = -1.0f + 2.0f * cy / height; projectionMatrix.m22 = -(far + near) / (far - near); projectionMatrix.m23 = -2.0f * far * near / (far - near); projectionMatrix.m32 = -1.0f; return projectionMatrix; } /// /// Calculate camera matrix values from projection matrix. (OpenGL coordinate system to OpenCV coordinate system) /// /// Projection matrix. /// Image width. /// Image height. /// Vertical field of view. /// /// Camera matrix values. (fx = matrx.m00, fy = matrx.m11, cx = matrx.m02, cy = matrx.m12) /// public static Matrix4x4 CameraMatrixValuesFromCalculateProjectionMatrix(Matrix4x4 projectionMatrix, float width, float height, float fovV) { float fovH = 2.0f * Mathf.Atan(width / height * Mathf.Tan(fovV * Mathf.Deg2Rad / 2.0f)) * Mathf.Rad2Deg; Matrix4x4 cameraMatrix = new Matrix4x4(); cameraMatrix.m00 = CalculateDistance(width, fovH); cameraMatrix.m02 = -((projectionMatrix.m02 * width - width) / 2); cameraMatrix.m11 = CalculateDistance(height, fovV); cameraMatrix.m12 = (projectionMatrix.m12 * height + height) / 2; cameraMatrix.m22 = 1.0f; return cameraMatrix; } /// /// Calculate frustum size. /// https://docs.unity3d.com/Manual/FrustumSizeAtDistance.html /// /// Distance. /// Field of view. (horizontal or vertical direction) /// /// Frustum height. /// public static float CalculateFrustumSize(float distance, float fov) { return 2.0f * distance * Mathf.Tan(fov * 0.5f * Mathf.Deg2Rad); } /// /// Calculate distance. /// https://docs.unity3d.com/Manual/FrustumSizeAtDistance.html /// /// One side size of a frustum. /// Field of view. (horizontal or vertical direction) /// /// Distance. /// public static float CalculateDistance(float frustumSize, float fov) { return frustumSize * 0.5f / Mathf.Tan(fov * 0.5f * Mathf.Deg2Rad); } /// /// Calculate FOV angle. /// https://docs.unity3d.com/Manual/FrustumSizeAtDistance.html /// /// One side size of a frustum. /// Distance. /// /// FOV angle. /// public static float CalculateFOVAngle(float frustumSize, float distance) { return 2.0f * Mathf.Atan(frustumSize * 0.5f / distance) * Mathf.Rad2Deg; } } }