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