/// <summary> /// Performs a lowpass check on the position and rotation of each marker in newDict, comparing them to those in oldDict. /// </summary> /// <param name="oldDict">Old dictionary.</param> /// <param name="newDict">New dictionary.</param> /// <param name="posThreshold">Positon threshold.</param> /// <param name="rotThreshold">Rotation threshold.</param> public static void LowpassPoseDataDict(Dictionary <int, PoseData> oldDict, Dictionary <int, PoseData> newDict, float posThreshold, float rotThreshold) { posThreshold *= posThreshold; List <int> keys = new List <int> (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; } }
/// <summary> /// Creates pose data dictionary. /// </summary> /// <param name="markerCount">Marker count.</param> /// <param name="ids">ids.</param> /// <param name="rvecs">Rvecs.</param> /// <param name="tvecs">Tvecs.</param> /// <returns>PoseData dictionary.</returns> public static Dictionary <int, PoseData> CreatePoseDataDict(int markerCount, int[] ids, double[] rvecs, double[] tvecs) { Dictionary <int, PoseData> dict = new Dictionary <int, PoseData> (); if (markerCount == 0) { return(dict); } Vector3 rvec = new Vector3(); for (int i = 0; i < markerCount; 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); }
/// <summary> /// Convertes rvec and tvec value to PoseData. /// </summary> /// <param name="tvec">Rvec.</param> /// <param name="tvec">Tvec.</param> /// <returns>PoseData.</returns> public static PoseData ConvertRvecTvecToPoseData(IList <double> rvec, IList <double> tvec) { PoseData data = new PoseData(); data.pos = ConvertTvecToPos(tvec); data.rot = ConvertRvecToRot(rvec); return(data); }
/// <summary> /// Convertes transform matrix to PoseData. /// </summary> /// <param name="matrix">Transform matrix.</param> /// <returns>PoseData.</returns> public static PoseData ConvertMatrixToPoseData(ref Matrix4x4 matrix) { PoseData data = new PoseData(); data.pos = ExtractTranslationFromMatrix(ref matrix); data.rot = ExtractRotationFromMatrix(ref matrix); return(data); }
/// <summary> /// Convertes rvec and tvec value to PoseData. /// </summary> /// <param name="tvec">Rvec.</param> /// <param name="tvec">Tvec.</param> /// <returns>PoseData.</returns> public static PoseData ConvertRvecTvecToPoseData(double[] rvec, double[] tvec) { PoseData data = new PoseData(); data.pos = ConvertTvecToPos(tvec); data.rot = ConvertRvecToRot(rvec); return(data); }
/// <summary> /// Convertes PoseData to transform matrix. /// </summary> /// <param name="posedata">PoseData.</param> /// <param name="toLeftHandCoordinateSystem">Determines if convert the transformation matrix to the left-hand coordinate system.</param> /// <returns>Transform matrix.</returns> 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); }
/// <summary> /// Performs a lowpass check on the position and rotation in newPose, comparing them to oldPose. /// </summary> /// <param name="oldPose">Old PoseData.</param> /// <param name="newPose">New PoseData.</param> /// <param name="posThreshold">Positon threshold.</param> /// <param name="rotThreshold">Rotation threshold.</param> 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; } }
/// <summary> /// Convertes PoseData to transform matrix. /// </summary> /// <param name="posedata">PoseData.</param> /// <param name="toLeftHandCoordinateSystem">Determines if convert the transformation matrix to the left-hand coordinate system.</param> /// <param name="invertZAxis">Determines if invert Z axis of the transform matrix.</param> /// <returns>Transform matrix.</returns> public static Matrix4x4 ConvertPoseDataToMatrix(ref PoseData poseData, bool toLeftHandCoordinateSystem = false, bool invertZAxis = false) { Matrix4x4 matrix = Matrix4x4.TRS(poseData.pos, poseData.rot, vector_one); // right-handed coordinates system (OpenCV) to left-handed one (Unity) if (toLeftHandCoordinateSystem) { matrix = invertYMatrix * matrix; } // Apply Z axis inverted matrix. if (invertZAxis) { matrix = matrix * invertZMatrix; } return(matrix); }
/// <summary> /// Creates pose data dictionary. /// </summary> /// <param name="ids">ids.</param> /// <param name="rvecs">Rvecs.</param> /// <param name="tvecs">Tvecs.</param> /// <returns>PoseData dictionary.</returns> public static Dictionary <int, PoseData> CreatePoseDataDict(IList <int> ids, IList <double> rvecs, IList <double> tvecs) { if (ids == null) { throw new ArgumentNullException("ids"); } if (rvecs == null) { throw new ArgumentNullException("rvecs"); } if (tvecs == null) { throw new ArgumentNullException("tvecs"); } Dictionary <int, PoseData> dict = new Dictionary <int, PoseData>(); 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); }