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);
}
}
}
