/// <summary> /// For Unity Simulator /// Saves the current camera pose into the struct simCameraPoses /// if the current pose is different (above threshold) from the previous pose /// </summary> /// <param name="currCameraPose">Curr camera pose.</param> public void SaveCameraPose() { PNTransformUnity currCameraPose = GetPose(); /// Converts PNTransformUnity back into Vector3 and Quaternion Vector3 currPosition = new Vector3(currCameraPose.position.x, currCameraPose.position.y, currCameraPose.position.z); Quaternion currRotation = new Quaternion(currCameraPose.rotation.x, currCameraPose.rotation.y, currCameraPose.rotation.z, currCameraPose.rotation.w); /// If the cameraPoses list is empty if (simCameraPoses.cameraPoses.Count == 0) { simCameraPoses.cameraPoses.Add(currCameraPose); } else { /// Get previous cameraPose from list PNTransformUnity prevCameraPose = simCameraPoses.cameraPoses[simCameraPoses.cameraPoses.Count - 1]; Vector3 prevPosition = new Vector3(prevCameraPose.position.x, prevCameraPose.position.y, prevCameraPose.position.z); Quaternion prevRotation = new Quaternion(prevCameraPose.rotation.x, prevCameraPose.rotation.y, prevCameraPose.rotation.z, prevCameraPose.rotation.w); float positionDiffNorm = Vector3.Distance(currPosition, prevPosition); float angleDiffNorm = Quaternion.Angle(prevRotation, currRotation); // Save current cameraPose as new pose if above distance and angle threshold if (positionDiffNorm > SIM_MAP_DISTANCE_THRESHOLD || angleDiffNorm > SIM_MAP_ANGLE_THRESHOLD) { simCameraPoses.cameraPoses.Add(currCameraPose); } } }
/// <summary> /// For Unity Simulator /// Checks if the current camera pose is within the range for localization. /// </summary> public void checkLocalization() { PNTransformUnity currCameraPose = GetPose(); Vector3 currPosition = new Vector3(currCameraPose.position.x, currCameraPose.position.y, currCameraPose.position.z); Quaternion currRotation = new Quaternion(currCameraPose.rotation.x, currCameraPose.rotation.y, currCameraPose.rotation.z, currCameraPose.rotation.w); /// Iterate through each saved cameraPose in the map to find the one that matches /// the current cameraPose. for (int i = 0; i < simCameraPoses.cameraPoses.Count; i++) { PNTransformUnity localizeCameraPose = simCameraPoses.cameraPoses[i]; Vector3 localizePosition = new Vector3(localizeCameraPose.position.x, localizeCameraPose.position.y, localizeCameraPose.position.z); Quaternion localizeRotation = new Quaternion(localizeCameraPose.rotation.x, localizeCameraPose.rotation.y, localizeCameraPose.rotation.z, localizeCameraPose.rotation.w); float positionDiffNorm = Vector3.Distance(currPosition, localizePosition); float angleDiffNorm = Quaternion.Angle(localizeRotation, currRotation); if (positionDiffNorm < SIM_LOCAL_DISTANCE_THRESHOLD && angleDiffNorm < SIM_LOCAL_ANGLE_THRESHOLD) { mCurrStatus = MappingStatus.RUNNING; break; } else { mCurrStatus = MappingStatus.LOST; } } }
static void OnPose(ref PNTransformUnity outputPose, ref PNTransformUnity arkitPose, IntPtr context) { Matrix4x4 outputPoseMat = PNUtility.MatrixOps.PNPose2Matrix4x4(outputPose); Matrix4x4 arkitPoseMat = PNUtility.MatrixOps.PNPose2Matrix4x4(arkitPose); MappingStatus status = Instance.GetStatus(); var listeners = Instance.listeners; if (status == MappingStatus.RUNNING) { MainThreadTaskQueue.InvokeOnMainThread(() => { foreach (var listener in listeners) { listener.OnPose(outputPoseMat, arkitPoseMat); } }); Instance.mCurrentTransform = outputPoseMat * arkitPoseMat.inverse; } if (status != Instance.mPrevStatus) { MainThreadTaskQueue.InvokeOnMainThread(() => { foreach (var listener in listeners) { listener.OnStatusChange(Instance.mPrevStatus, status); } Instance.mPrevStatus = status; }); } }
/// <summary> /// For Unity Simulator /// A coroutine that calls OnPose with in 0.5s intervals /// Designed to mimick the behaviour of Invoke Repeating /// </summary> IEnumerator OnPoseInvokeRepeat() { while (true) { PNTransformUnity currCameraPose = GetPose(); OnPose(ref currCameraPose, ref currCameraPose, IntPtr.Zero); yield return(new WaitForSeconds(0.5f)); } }
/// <summary> /// Gets the current pose computed by the mapping session /// </summary> /// <returns>The current pose computed by the mapping session</returns> public PNTransformUnity GetPose() { PNTransformUnity result = new PNTransformUnity(); #if !UNITY_EDITOR PNGetPose(ref result); #endif return(result); }
/// <summary> /// Gets the current pose computed by the mapping session /// </summary> /// <returns>The current pose computed by the mapping session</returns> public PNTransformUnity GetPose() { PNTransformUnity result = new PNTransformUnity(); #if !UNITY_EDITOR PNGetPose(ref result); #else /// Manually setting result to current Unity camera pose result.position.x = Camera.main.gameObject.transform.position.x; result.position.y = Camera.main.gameObject.transform.position.y; result.position.z = Camera.main.gameObject.transform.position.z; result.rotation.x = Camera.main.gameObject.transform.rotation.x; result.rotation.y = Camera.main.gameObject.transform.rotation.y; result.rotation.z = Camera.main.gameObject.transform.rotation.z; result.rotation.w = Camera.main.gameObject.transform.rotation.w; #endif return(result); }
/// <summary> /// Sends an image frame and its corresponding camera pose to LibPlacenote mapping/localization module /// </summary> /// <param name="frameData">Image frame data.</param> /// <param name="position">Position of the camera at the time frameData is captured</param> /// <param name="rotation">Quaternion of the camera at the time frameData is captured.</param> /// <param name="screenOrientation"> /// Fill in this parameter with screenOrientation from the current UnityVideoParams structure. /// Used to correct for the extra rotation applied by the Unity ARKit Plugin on the ARKit pose transform. /// </param> public void SendARFrame(UnityARImageFrameData frameData, Vector3 position, Quaternion rotation, int screenOrientation) { Matrix4x4 orientRemovalMat = Matrix4x4.zero; orientRemovalMat.m22 = orientRemovalMat.m33 = 1; switch (screenOrientation) { // portrait case 1: orientRemovalMat.m01 = 1; orientRemovalMat.m10 = -1; break; case 2: orientRemovalMat.m01 = -1; orientRemovalMat.m10 = 1; break; // landscape case 3: // do nothing orientRemovalMat = Matrix4x4.identity; break; case 4: orientRemovalMat.m00 = -1; orientRemovalMat.m11 = -1; break; default: Debug.LogError("Unrecognized screen orientation"); return; } Matrix4x4 rotationMat = Matrix4x4.TRS(new Vector3(0, 0, 0), rotation, new Vector3(1, 1, 1)); rotationMat = rotationMat * orientRemovalMat; rotation = PNUtility.MatrixOps.QuaternionFromMatrix(rotationMat); PNTransformUnity pose = new PNTransformUnity(); pose.position.x = position.x; pose.position.y = position.y; pose.position.z = position.z; pose.rotation.x = rotation.x; pose.rotation.y = rotation.y; pose.rotation.z = rotation.z; pose.rotation.w = rotation.w; PNImagePlaneUnity yPlane = new PNImagePlaneUnity(); yPlane.width = (int)frameData.y.width; yPlane.height = (int)frameData.y.height; yPlane.stride = (int)frameData.y.stride; yPlane.buf = frameData.y.data; PNImagePlaneUnity vuPlane = new PNImagePlaneUnity(); vuPlane.width = (int)frameData.vu.width; vuPlane.height = (int)frameData.vu.height; vuPlane.stride = (int)frameData.vu.stride; vuPlane.buf = frameData.vu.data; #if !UNITY_EDITOR PNSetFrame(ref yPlane, ref vuPlane, ref pose); #endif }
private static extern int PNGetPose(ref PNTransformUnity pose);
private static extern void PNSetFrame( ref PNImagePlaneUnity yPlane, ref PNImagePlaneUnity vuPlane, ref PNTransformUnity pose );