/// <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;
}
}
private void UpdateARObjectTransform(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);
}
}
/// <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 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>
/// 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);
}
