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