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
{
Vector4 intrinsics;
Camera cam = this.mainCamera;
Vector3 camPos = cam.transform.position;
Quaternion camRot = cam.transform.rotation;
int param1 = mapperSettings.param1;
int param2 = mapperSettings.param2;
float param3 = mapperSettings.param3;
float param4 = mapperSettings.param4;
int method = mapperSettings.localizer;
ARHelper.GetIntrinsics(out intrinsics);
ARHelper.GetPlaneDataFast(ref m_PixelBuffer, image);
if (m_PixelBuffer != IntPtr.Zero)
{
Vector3 position = Vector3.zero;
Quaternion rotation = Quaternion.identity;
Task <int> t = Task.Run(() =>
{
return(Immersal.Core.LocalizeImage(out position, out rotation, image.width, image.height, ref intrinsics, m_PixelBuffer, param1, param2, param3, param4, method));
});
await t;
int mapHandle = t.Result;
if (mapHandle >= 0)
{
this.stats.locSucc++;
Debug.Log("*************************** Localization Succeeded ***************************");
Debug.Log(string.Format("params: {0}, {1}, {2}, {3}", param1, param2, param3, param4));
Matrix4x4 cloudSpace = Matrix4x4.TRS(position, rotation, Vector3.one);
Matrix4x4 trackerSpace = Matrix4x4.TRS(camPos, camRot, Vector3.one);
Debug.Log("handle " + mapHandle + "\n" +
"fc 4x4\n" + cloudSpace + "\n" +
"ft 4x4\n" + trackerSpace);
Matrix4x4 m = trackerSpace * (cloudSpace.inverse);
LocalizerPose lastLocalizedPose;
LocalizerBase.GetLocalizerPose(out lastLocalizedPose, mapHandle, position, rotation, m.inverse);
this.lastLocalizedPose = lastLocalizedPose;
foreach (PointCloudRenderer p in this.pcr.Values)
{
if (p.mapHandle == mapHandle)
{
p.go.transform.position = m.GetColumn(3);
p.go.transform.rotation = m.rotation;
break;
}
}
}
else
{
this.stats.locFail++;
Debug.Log("*************************** Localization Failed ***************************");
}
}
image.Dispose();
}
}
