public int LoadMap(byte[] mapBytes = null)
{
if (mapBytes == null)
{
mapBytes = (mapFile != null) ? mapFile.bytes : null;
}
if (mapBytes == null)
{
return(-1);
}
m_MapId = Immersal.Core.LoadMap(mapBytes);
if (m_MapId != -1)
{
Vector3[] points = new Vector3[65536];
int num = Immersal.Core.GetPointCloud(m_MapId, points);
CreateCloud(points, num);
if (!Application.isEditor)
{
m_Root = m_ARSpace.transform;
Matrix4x4 offset = Matrix4x4.TRS(transform.localPosition, transform.localRotation, Vector3.one);
ARSpace.RegisterSpace(m_Root, m_MapId, offset);
}
}
return(m_MapId);
}
public int LoadMap(byte[] mapBytes = null)
{
if (mapBytes == null)
{
mapBytes = (mapFile != null) ? mapFile.bytes : null;
}
if (mapBytes != null && mapId <= 0)
{
mapId = Immersal.Core.LoadMap(mapBytes);
}
if (mapId > 0)
{
Vector3[] points = new Vector3[MAX_VERTICES];
int num = Immersal.Core.GetPointCloud(mapId, points);
CreateCloud(points, num);
root = m_ARSpace.transform;
ARSpace.RegisterSpace(root, mapId, transform.localPosition, transform.localRotation, transform.localScale);
}
return(mapId);
}
public void FreeMap()
{
if (mapId > 0)
{
Immersal.Core.FreeMap(mapId);
ARSpace.UnregisterSpace(root, mapId);
mapId = -1;
}
}
void Awake()
{
m_ARSpace = gameObject.GetComponentInParent <ARSpace>();
if (!m_ARSpace)
{
GameObject go = new GameObject("AR Space");
m_ARSpace = go.AddComponent <ARSpace>();
transform.SetParent(go.transform);
}
}
public void FreeMap()
{
if (m_MapId != -1)
{
Immersal.Core.FreeMap(m_MapId);
if (!Application.isEditor)
{
ARSpace.UnregisterSpace(root, m_MapId);
}
}
}
private void Start()
{
InitMesh();
ARSpace space = gameObject.GetComponentInParent <ARSpace>();
if (!space)
{
GameObject go = new GameObject("AR Space");
space = go.AddComponent <ARSpace>();
transform.SetParent(go.transform);
}
m_MapHandle = LoadMap();
if (m_MapHandle != -1)
{
m_Root = space.transform;
Matrix4x4 offset = Matrix4x4.TRS(transform.localPosition, transform.localRotation, new Vector3(1f, 1f, 1f));
Immersal.AR.ARLocalizer.RegisterSpace(m_Root, m_MapHandle, offset);
}
}
public override async void Localize()
{
#if PLATFORM_LUMIN
XRCameraImage image;
#else
XRCpuImage image;
#endif
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
#if PLATFORM_LUMIN
if (cameraSubsystem != null && cameraSubsystem.TryGetLatestImage(out image))
#else
if (cameraSubsystem != null && cameraSubsystem.TryAcquireLatestCpuImage(out image))
#endif
{
stats.localizationAttemptCount++;
Vector4 intrinsics;
Vector3 camPos = m_Cam.transform.position;
Quaternion camRot = m_Cam.transform.rotation;
ARHelper.GetIntrinsics(out intrinsics);
ARHelper.GetPlaneDataFast(ref m_PixelBuffer, image);
if (m_PixelBuffer != IntPtr.Zero)
{
Vector3 pos = Vector3.zero;
Quaternion rot = Quaternion.identity;
float startTime = Time.realtimeSinceStartup;
Task <int> t = Task.Run(() =>
{
return(Immersal.Core.LocalizeImage(out pos, out rot, image.width, image.height, ref intrinsics, m_PixelBuffer));
});
await t;
int mapHandle = t.Result;
float elapsedTime = Time.realtimeSinceStartup - startTime;
if (mapHandle >= 0 && ARSpace.mapHandleToOffset.ContainsKey(mapHandle))
{
Debug.Log(string.Format("Relocalized in {0} seconds", elapsedTime));
stats.localizationSuccessCount++;
if (mapHandle != lastLocalizedMapHandle)
{
if (resetOnMapChange)
{
Reset();
}
lastLocalizedMapHandle = mapHandle;
OnMapChanged?.Invoke(mapHandle);
}
ARHelper.GetRotation(ref rot);
MapOffset mo = ARSpace.mapHandleToMap.ContainsKey(mapHandle) ? ARSpace.mapHandleToOffset[mapHandle] : null;
if (mo == null)
{
return;
}
Matrix4x4 offsetNoScale = Matrix4x4.TRS(mo.position, mo.rotation, Vector3.one);
Vector3 scaledPos = new Vector3
(
pos.x * mo.scale.x,
pos.y * mo.scale.y,
pos.z * mo.scale.z
);
Matrix4x4 cloudSpace = offsetNoScale * Matrix4x4.TRS(scaledPos, rot, Vector3.one);
Matrix4x4 trackerSpace = Matrix4x4.TRS(camPos, camRot, Vector3.one);
Matrix4x4 m = trackerSpace * (cloudSpace.inverse);
if (useFiltering)
{
mo.space.filter.RefinePose(m);
}
else
{
ARSpace.UpdateSpace(mo.space, m.GetColumn(3), m.rotation);
}
LocalizerPose localizerPose;
GetLocalizerPose(out localizerPose, mapHandle, pos, rot, m.inverse);
OnPoseFound?.Invoke(localizerPose);
if (ARSpace.mapHandleToMap.ContainsKey(mapHandle))
{
ARMap map = ARSpace.mapHandleToMap[mapHandle];
map.NotifySuccessfulLocalization(mapHandle);
}
}
else
{
Debug.Log(string.Format("Localization attempt failed after {0} seconds", elapsedTime));
}
}
image.Dispose();
}
base.Localize();
}
public override async void LocalizeServer(SDKMapId[] mapIds)
{
#if PLATFORM_LUMIN
XRCameraImage image;
#else
XRCpuImage image;
#endif
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
#if PLATFORM_LUMIN
if (cameraSubsystem.TryGetLatestImage(out image))
#else
if (cameraSubsystem.TryAcquireLatestCpuImage(out image))
#endif
{
stats.localizationAttemptCount++;
JobLocalizeServerAsync j = new JobLocalizeServerAsync();
byte[] pixels;
Camera cam = Camera.main;
Vector3 camPos = cam.transform.position;
Quaternion camRot = cam.transform.rotation;
Vector4 intrinsics;
int channels = 1;
int width = image.width;
int height = image.height;
ARHelper.GetIntrinsics(out intrinsics);
ARHelper.GetPlaneData(out pixels, image);
float startTime = Time.realtimeSinceStartup;
Task <(byte[], icvCaptureInfo)> t = Task.Run(() =>
{
byte[] capture = new byte[channels * width * height + 1024];
icvCaptureInfo info = Immersal.Core.CaptureImage(capture, capture.Length, pixels, width, height, channels);
Array.Resize(ref capture, info.captureSize);
return(capture, info);
});
await t;
j.image = t.Result.Item1;
j.position = camPos;
j.rotation = camRot;
j.intrinsics = intrinsics;
j.mapIds = mapIds;
j.OnResult += async(SDKLocalizeResult result) =>
{
float elapsedTime = Time.realtimeSinceStartup - startTime;
if (result.success)
{
Debug.Log("*************************** On-Server Localization Succeeded ***************************");
Debug.Log(string.Format("Relocalized in {0} seconds", elapsedTime));
int mapServerId = result.map;
if (mapServerId > 0 && ARSpace.mapHandleToOffset.ContainsKey(mapServerId))
{
if (mapServerId != lastLocalizedMapHandle)
{
if (resetOnMapChange)
{
Reset();
}
lastLocalizedMapHandle = mapServerId;
OnMapChanged?.Invoke(mapServerId);
}
MapOffset mo = ARSpace.mapHandleToOffset[mapServerId];
stats.localizationSuccessCount++;
Matrix4x4 responseMatrix = Matrix4x4.identity;
responseMatrix.m00 = result.r00; responseMatrix.m01 = result.r01; responseMatrix.m02 = result.r02; responseMatrix.m03 = result.px;
responseMatrix.m10 = result.r10; responseMatrix.m11 = result.r11; responseMatrix.m12 = result.r12; responseMatrix.m13 = result.py;
responseMatrix.m20 = result.r20; responseMatrix.m21 = result.r21; responseMatrix.m22 = result.r22; responseMatrix.m23 = result.pz;
Vector3 pos = responseMatrix.GetColumn(3);
Quaternion rot = responseMatrix.rotation;
ARHelper.GetRotation(ref rot);
Matrix4x4 offsetNoScale = Matrix4x4.TRS(mo.position, mo.rotation, Vector3.one);
Vector3 scaledPos = Vector3.Scale(pos, mo.scale);
Matrix4x4 cloudSpace = offsetNoScale * Matrix4x4.TRS(scaledPos, rot, Vector3.one);
Matrix4x4 trackerSpace = Matrix4x4.TRS(camPos, camRot, Vector3.one);
Matrix4x4 m = trackerSpace * (cloudSpace.inverse);
if (useFiltering)
{
mo.space.filter.RefinePose(m);
}
else
{
ARSpace.UpdateSpace(mo.space, m.GetColumn(3), m.rotation);
}
JobEcefAsync je = new JobEcefAsync();
je.id = mapServerId;
je.OnResult += (SDKEcefResult result2) =>
{
LocalizerPose localizerPose;
LocalizerBase.GetLocalizerPose(out localizerPose, mapServerId, pos, rot, m.inverse, result2.ecef);
OnPoseFound?.Invoke(localizerPose);
};
await je.RunJobAsync();
if (ARSpace.mapHandleToMap.ContainsKey(mapServerId))
{
ARMap map = ARSpace.mapHandleToMap[mapServerId];
map.NotifySuccessfulLocalization(mapServerId);
}
}
else
{
Debug.Log(string.Format("Localization attempt failed after {0} seconds", elapsedTime));
}
}
else
{
Debug.Log("*************************** On-Server Localization Failed ***************************");
}
};
await j.RunJobAsync();
image.Dispose();
}
base.LocalizeServer(mapIds);
}
protected virtual void Update()
{
if (!m_LocalizeContinuously)
{
return;
}
if (ARSpace.transformToSpace.Count == 0)
{
m_BurstStartTime = Time.unscaledTime;
return;
}
if (useFiltering)
{
foreach (KeyValuePair <Transform, SpaceContainer> item in ARSpace.transformToSpace)
{
float distSq = (item.Value.filter.position - item.Value.targetPosition).sqrMagnitude;
float cosAngle = Quaternion.Dot(item.Value.filter.rotation, item.Value.targetRotation);
if (item.Value.filter.SampleCount() == 1 || distSq > m_WarpThresholdDistSq || cosAngle < m_WarpThresholdCosAngle)
{
item.Value.targetPosition = item.Value.filter.position;
item.Value.targetRotation = item.Value.filter.rotation;
}
else
{
float smoothing = 0.025f;
float steps = Time.deltaTime / (1.0f / 60.0f);
if (steps < 1.0f)
{
steps = 1.0f;
}
else if (steps > 6.0f)
{
steps = 6.0f;
}
float alpha = 1.0f - Mathf.Pow(1.0f - smoothing, steps);
item.Value.targetRotation = Quaternion.Slerp(item.Value.targetRotation, item.Value.filter.rotation, alpha);
item.Value.targetPosition = Vector3.Lerp(item.Value.targetPosition, item.Value.filter.position, alpha);
}
ARSpace.UpdateSpace(item.Value, item.Value.targetPosition, item.Value.targetRotation);
}
}
float curTime = Time.unscaledTime;
if (m_BurstModeActive) // try to localize at max speed during app start/resume
{
if (!isLocalizing && isTracking)
{
float elapsedTime = curTime - m_BurstStartTime;
isLocalizing = true;
if (useServerLocalizer && serverMapIds.Length > 0)
{
LocalizeServer(serverMapIds);
}
else
{
Localize();
}
if (stats.localizationSuccessCount == 10 || elapsedTime >= 15f)
{
m_BurstModeActive = false;
}
}
}
if (!isLocalizing && isTracking && (curTime - m_LastLocalizeTime) >= localizationInterval)
{
m_LastLocalizeTime = curTime;
isLocalizing = true;
if (useServerLocalizer && serverMapIds.Length > 0)
{
LocalizeServer(serverMapIds);
}
else
{
Localize();
}
}
}
