private void OnArucoPose(MarkerPose pose)
{
var sceneCamTransform = SceneCameraTracker.Instance.transform;
int index = -1;
if (pose.Id == TopLeftId)
{
index = TOPLEFT;
}
else if (pose.Id == BottomRightId)
{
index = BOTTOMRIGHT;
}
else if (pose.Id == TopRightId)
{
index = TOPRIGHT;
}
else if (pose.Id == BottomLeftId)
{
index = BOTTOMLEFT;
}
else
{
return;
}
var worldPos = sceneCamTransform.TransformPoint(pose.Position);
var worldRot = sceneCamTransform.rotation * pose.Rotation;
_poses[index].AddSample(worldPos, worldRot);
}
private void OnArPose(MarkerPose pose)
{
if (pose.Id == Id)
{
Matrix4x4 marker = Matrix4x4.TRS(pose.Position, pose.Rotation, Vector3.one);
Matrix4x4 cam = marker.inverse;
HasDetectedCamera = true;
DetectedCameraPosition = transform.position + transform.rotation * cam.GetPosition();
DetectedCameraRotation = transform.rotation * cam.GetRotation();
CameraDetectionTime = Time.unscaledTime;
Confidence = (float)pose.Confidence;
}
}
void OnArucoPose(MarkerPose pose)
{
if (pose.Id == MarkerId)
{
// pose is marker's pose -> inverted we get camera pose
var markerMatrix = Matrix4x4.TRS(pose.Position, pose.Rotation, Vector3.one);
var cameraMatrix = markerMatrix.inverse;
var cameraLocalPos = cameraMatrix.GetPosition();
var cameraWorldPos = transform.TransformPoint(cameraLocalPos);
var camPose = new TimedPose
{
DetectionTime = DateTime.Now,
CameraPose = cameraWorldPos
};
_savedPoses.Add(camPose);
}
}
private void OnArucoPose(MarkerPose pose)
{
if (pose.Id == ArucoCalibrationId)
{
// we're interested in camera's position relative to marker, not markerposition
// -> we can get camera position by inverting marker transformation matrix
var transformMatrix = Matrix4x4.TRS(pose.Position, pose.Rotation, Vector3.one);
var invMatrix = transformMatrix.inverse;
var prevPos = _arucoPos;
var prevRot = _arucoRot;
_arucoPos = invMatrix.GetPosition();
_arucoRot = invMatrix.GetRotation();
var hasSteadyPos = (_arucoPos - prevPos).sqrMagnitude < SteadyPosThreshold;
var hasSteadyRot = Quaternion.Angle(prevRot, _arucoRot) < SteadyAngleThreshold;
HasSteadyArucoPose = hasSteadyPos && hasSteadyRot;
}
}
private void OnArtkPose(MarkerPose pose)
{
if (pose.Name == ArtkCalibrationName)
{
// we're interested in camera's position relative to marker, not markerposition
// -> we can get camera position by inverting marker transformation matrix
var invertedPose = pose.Inverse();
var prevPos = _artkPos;
var prevRot = _artkRot;
_artkPos = invertedPose.Position;
_artkRot = invertedPose.Rotation;
var hasSteadyPos = (_artkPos - prevPos).sqrMagnitude < SteadyPosThreshold;
var hasSteadyRot = Quaternion.Angle(prevRot, _artkRot) < SteadyAngleThreshold;
HasSteadyArtkPose = hasSteadyPos && hasSteadyRot;
}
}
private void OnArucoPose(MarkerPose pose)
{
foreach (var cMarker in _markerSetupScript.CalibratedMarkers)
{
if (cMarker.Id == pose.Id)
{
// pose is marker's pose -> inverted we get camera pose
var markerMatrix = Matrix4x4.TRS(pose.Position, pose.Rotation, Vector3.one);
var cameraMatrix = markerMatrix.inverse;
var cameraLocalPos = cameraMatrix.GetPosition();
var cameraWorldPos = cMarker.Marker.transform.TransformPoint(cameraLocalPos);
var cameraLocalRot = cameraMatrix.GetRotation();
var cameraWorldForward = cMarker.Marker.transform.TransformDirection(cameraLocalRot * Vector3.forward);
var cameraWorldUp = cMarker.Marker.transform.TransformDirection(cameraLocalRot * Vector3.up);
var cameraWorldRot = Quaternion.LookRotation(cameraWorldForward, cameraWorldUp);
var calibratedPose = new Pose
{
Id = pose.Id,
Position = cameraWorldPos,
Rotation = cameraWorldRot
};
if (_calibratedArucoPoses.ContainsKey(pose.Id))
{
_calibratedArucoPoses[pose.Id] = calibratedPose;
}
else
{
_calibratedArucoPoses.Add(pose.Id, calibratedPose);
}
// TODO thresholding etc
HasSteadyArucoPose = true;
break;
}
}
}
void OnNewPose(MarkerPose pose)
{
if (pose.Id == TrackedId)
{
if (Invert)
{
Matrix4x4 marker = Matrix4x4.TRS(pose.Position, pose.Rotation, Vector3.one);
Matrix4x4 cam = marker.inverse;
transform.localPosition = cam.GetPosition();
transform.localRotation = cam.GetRotation();
}
else
{
//transform.localPosition = pose.Position;
//transform.localRotation = pose.Rotation;
var camPos = SceneCameraTracker.Instance.transform.position;
var camRot = SceneCameraTracker.Instance.transform.rotation;
transform.position = camPos + camRot * pose.Position + camRot * new Vector3(0, transform.localScale.y, 0) / 2f;
transform.rotation = camRot * pose.Rotation;
}
}
}
void OnNewPose(MarkerPose pose)
{
if (pose.Id == TrackedId)
{
if (Invert)
{
Matrix4x4 marker = Matrix4x4.TRS(pose.Position, pose.Rotation, Vector3.one);
Matrix4x4 cam = marker.inverse;
transform.localPosition = cam.GetPosition();
transform.localRotation = cam.GetRotation();
}
else
{
//transform.localPosition = pose.Position;
//transform.localRotation = pose.Rotation;
var camPos = SceneCameraTracker.Instance.transform.position;
var camRot = SceneCameraTracker.Instance.transform.rotation;
transform.position = camPos + camRot * pose.Position + camRot * new Vector3(0, transform.localScale.y, 0) / 2f;
transform.rotation = camRot * pose.Rotation;
}
}
}
private void OnArucoPose(MarkerPose pose)
{
var markerOffset = CalibrationOffsets[pose.Id];
if (markerOffset == null)
{
markerOffset = new MarkerOffset
{
ArMarkerId = pose.Id
};
CalibrationOffsets[pose.Id] = markerOffset;
}
// pose is marker's pose -> inverted we get camera pose
var markerMatrix = Matrix4x4.TRS(pose.Position, pose.Rotation, Vector3.one);
var cameraMatrix = markerMatrix.inverse;
markerOffset.HasArPose = true;
markerOffset.ArPoseDetectionTime = Time.unscaledTime;
markerOffset.ArMarkerPosition = pose.Position;
markerOffset.ArMarkerRotation = pose.Rotation;
markerOffset.ArCameraPosition = cameraMatrix.GetPosition();
markerOffset.ArCameraRotation = cameraMatrix.GetRotation();
}
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);
}
}
}
