private static MarkerCorners CalculateMarkerCorners(Vector3 topLeftPosition, Quaternion topLeftOrientation, float size)
{
var corners = new MarkerCorners();
corners.topLeft = topLeftPosition;
var originToTopLeftCorner = Matrix4x4.TRS(topLeftPosition, topLeftOrientation, Vector3.one);
corners.topRight = originToTopLeftCorner.MultiplyPoint(new Vector3(-size, 0, 0));
corners.bottomLeft = originToTopLeftCorner.MultiplyPoint(new Vector3(0, -size, 0));
corners.bottomRight = originToTopLeftCorner.MultiplyPoint(new Vector3(-size, -size, 0));
corners.orientation = topLeftOrientation;
return(corners);
}
/// <summary>
/// Creates a list of marker pairs that are corrected for the unity camera location specified in the headset calibration data.
/// </summary>
/// <param name="data">Headset calibration data</param>
/// <returns>Marker corners transformed to correct for the unity camera orientation</returns>
public static List <MarkerCorners> CalcMarkerCornersRelativeToCamera(HeadsetCalibrationData data)
{
List <MarkerCorners> markersRelativeToCamera = new List <MarkerCorners>();
var cameraTransform = Matrix4x4.TRS(data.headsetData.position, data.headsetData.rotation, Vector3.one);
var inverseCameraTransform = cameraTransform.inverse;
foreach (var markerPair in data.markers)
{
var arUcoCorners = markerPair.arucoMarkerCorners;
var cornersRelativeToCamera = new MarkerCorners();
cornersRelativeToCamera.topLeft = inverseCameraTransform.MultiplyPoint(arUcoCorners.topLeft);
cornersRelativeToCamera.topRight = inverseCameraTransform.MultiplyPoint(arUcoCorners.topRight);
cornersRelativeToCamera.bottomRight = inverseCameraTransform.MultiplyPoint(arUcoCorners.bottomRight);
cornersRelativeToCamera.bottomLeft = inverseCameraTransform.MultiplyPoint(arUcoCorners.bottomLeft);
var orientationTransform = inverseCameraTransform * Matrix4x4.TRS(Vector3.zero, arUcoCorners.orientation, Vector3.one);
cornersRelativeToCamera.orientation = Quaternion.LookRotation(orientationTransform.GetColumn(2), orientationTransform.GetColumn(1));
markersRelativeToCamera.Add(cornersRelativeToCamera);
}
return(markersRelativeToCamera);
}