private void DoSingleMarkerCalibration(Surface surface) { // get nearest marker (from center) with up-to-date ar marker pose MarkerPose nearestMarker = null; var nearestDistance = 0f; // get intersection between camera ray and display plane Vector3 intersection = new Vector3(); var hasIntersection = MathUtility.LinePlaneIntersection(out intersection, TrackedCamera.position, TrackedCamera.forward, surface.Normal, surface.GetCornerPosition(Corner.TopLeft)); var angle = MathUtility.AngleVectorPlane(TrackedCamera.forward, surface.Normal); // use <, as we want the camera to be perpendicular to the table ( | camera, _ table) __camTableAngle = Mathf.Abs(angle); if (Mathf.Abs(angle) < MinMarkerAngle) { __hasWrongAngle = true; return; } __hasWrongAngle = false; if (hasIntersection) { __intersection = intersection; var poses = UseMaps ? ArucoMapListener.Instance.DetectedPoses.Values : ArMarkerTracker.Instance.DetectedPoses.Values; foreach (var marker in poses) { bool isCurrent = (Time.unscaledTime - marker.DetectionTime) < ArCutoffTime; if (!isCurrent) { continue; } // shave off a few calculations // calculate distance between intersection and marker var markerWorldPosition = GetMarkerWorldPosition(marker.Id); var distance = (intersection - markerWorldPosition).sqrMagnitude; bool isNearest = (nearestMarker == null) || ((distance < nearestDistance)); if (isCurrent && isNearest) { nearestMarker = marker; nearestDistance = distance; } } } if (nearestMarker != null && nearestDistance < NearestDistanceThreshold) { var markerWorldPos = GetMarkerWorldPosition(nearestMarker.Id); var distMarkerToCamera = (markerWorldPos - TrackedCamera.position).sqrMagnitude; __camMarkerDistance = distMarkerToCamera; if (distMarkerToCamera > MaxMarkerCameraDistance) { __isTooFarAway = true; return; } __isTooFarAway = false; if (__nearestMarkerId != nearestMarker.Id) { _perMarkerPosCalib.Clear(); _perMarkerRotCalib.Clear(); } // debugging __nearestMarkerId = nearestMarker.Id; // apply calibration var markerMatrix = Matrix4x4.TRS(nearestMarker.Position, nearestMarker.Rotation, Vector3.one); var cameraMatrix = markerMatrix.inverse; var localPos = cameraMatrix.GetPosition(); var worldPos = markerWorldPos + surface.Rotation * localPos; var localRot = cameraMatrix.GetRotation(); var localForward = localRot * Vector3.forward; var localUp = localRot * Vector3.up; var worldForward = surface.Rotation * localForward; var worldUp = surface.Rotation * localUp; var worldRot = Quaternion.LookRotation(worldForward, worldUp); _perMarkerPosCalib.Add(Quaternion.Inverse(_optitrackPose.Rotation) * (worldPos - _optitrackPose.Position)); _perMarkerRotCalib.Add(worldRot * Quaternion.Inverse(_ovrRotation)); if (_perMarkerPosCalib.Count > 5 && _perMarkerPosCalib.Count < 50) { CalibrationParams.PositionOffset = MathUtility.Average(_perMarkerPosCalib); // c = b * inv(a) // => b = c * a? // from ovrRot to worldRot CalibrationParams.RotationOffset = MathUtility.Average(_perMarkerRotCalib); } } }