public override IEnumerator RunJob()
{
Debug.Log("*************************** CoroutineJobLocalize On-Server ***************************");
BaseMapper mapper = host as BaseMapper;
byte[] capture = new byte[channels * width * height + 1024];
Task <(string, icvCaptureInfo)> t = Task.Run(() =>
{
icvCaptureInfo info = Immersal.Core.CaptureImage(capture, capture.Length, pixels, width, height, channels);
return(Convert.ToBase64String(capture, 0, info.captureSize), info);
});
while (!t.IsCompleted)
{
yield return(null);
}
string encodedImage = t.Result.Item1;
icvCaptureInfo captureInfo = t.Result.Item2;
SDKLocalizeRequest imageRequest = this.useGPS ? new SDKGeoLocalizeRequest() : new SDKLocalizeRequest();
imageRequest.token = mapper.token;
imageRequest.fx = intrinsics.x;
imageRequest.fy = intrinsics.y;
imageRequest.ox = intrinsics.z;
imageRequest.oy = intrinsics.w;
imageRequest.b64 = encodedImage;
if (this.useGPS)
{
SDKGeoLocalizeRequest gr = imageRequest as SDKGeoLocalizeRequest;
gr.latitude = this.latitude;
gr.longitude = this.longitude;
gr.radius = this.radius;
}
else
{
int n = mapper.pcr.Count;
imageRequest.mapIds = new SDKMapId[n];
int count = 0;
foreach (int id in mapper.pcr.Keys)
{
imageRequest.mapIds[count] = new SDKMapId();
imageRequest.mapIds[count++].id = id;
}
}
string jsonString = JsonUtility.ToJson(imageRequest);
string endpoint = this.useGPS ? Endpoint.SERVER_GEOLOCALIZE : Endpoint.SERVER_LOCALIZE;
SDKLocalizeResult locResult = new SDKLocalizeResult();
locResult.success = false;
Matrix4x4 m = new Matrix4x4();
Matrix4x4 cloudSpace = new Matrix4x4();
using (UnityWebRequest request = UnityWebRequest.Put(string.Format(Endpoint.URL_FORMAT, mapper.server, endpoint), jsonString))
{
request.method = UnityWebRequest.kHttpVerbPOST;
request.useHttpContinue = false;
request.SetRequestHeader("Content-Type", "application/json");
request.SetRequestHeader("Accept", "application/json");
yield return(request.SendWebRequest());
if (request.isNetworkError || request.isHttpError)
{
Debug.LogError(request.error);
}
else if (request.responseCode == (long)HttpStatusCode.OK)
{
locResult = JsonUtility.FromJson <SDKLocalizeResult>(request.downloadHandler.text);
if (locResult.success)
{
cloudSpace = Matrix4x4.identity;
cloudSpace.m00 = locResult.r00; cloudSpace.m01 = locResult.r01; cloudSpace.m02 = locResult.r02; cloudSpace.m03 = locResult.px;
cloudSpace.m10 = locResult.r10; cloudSpace.m11 = locResult.r11; cloudSpace.m12 = locResult.r12; cloudSpace.m13 = locResult.py;
cloudSpace.m20 = locResult.r20; cloudSpace.m21 = locResult.r21; cloudSpace.m22 = locResult.r22; cloudSpace.m23 = locResult.pz;
Matrix4x4 trackerSpace = Matrix4x4.TRS(position, rotation, Vector3.one);
mapper.stats.locSucc++;
Debug.Log("*************************** On-Server Localization Succeeded ***************************");
Debug.Log("fc 4x4\n" + cloudSpace + "\n" +
"ft 4x4\n" + trackerSpace);
m = trackerSpace * (cloudSpace.inverse);
foreach (KeyValuePair <int, PointCloudRenderer> p in mapper.pcr)
{
if (p.Key == locResult.map)
{
p.Value.go.transform.position = m.GetColumn(3);
p.Value.go.transform.rotation = m.rotation;
break;
}
}
Debug.Log(locResult.error);
}
else
{
mapper.stats.locFail++;
Debug.Log("*************************** On-Server Localization Failed ***************************");
}
}
}
if (locResult.success)
{
SDKEcefRequest ecefRequest = new SDKEcefRequest();
ecefRequest.token = mapper.token;
ecefRequest.id = locResult.map;
using (UnityWebRequest request = UnityWebRequest.Put(string.Format(Endpoint.URL_FORMAT, mapper.server, Endpoint.SERVER_ECEF), JsonUtility.ToJson(ecefRequest)))
{
request.method = UnityWebRequest.kHttpVerbPOST;
request.useHttpContinue = false;
request.SetRequestHeader("Content-Type", "application/json");
request.SetRequestHeader("Accept", "application/json");
yield return(request.SendWebRequest());
if (request.isNetworkError || request.isHttpError)
{
Debug.LogError(request.error);
}
else if (request.responseCode == (long)HttpStatusCode.OK)
{
SDKEcefResult result = JsonUtility.FromJson <SDKEcefResult>(request.downloadHandler.text);
Debug.Log(request.downloadHandler.text);
double[] wgs84 = new double[3];
Vector3 pos = cloudSpace.GetColumn(3);
Quaternion rot = cloudSpace.rotation;
int r = Immersal.Core.PosMapToWgs84(wgs84, pos, result.ecef);
mapper.vlatitude = wgs84[0];
mapper.vlongitude = wgs84[1];
mapper.valtitude = wgs84[2];
if (r == 0)
{
mapper.lastLocalizedPose = (true, result.ecef, m.inverse, new Pose(pos, rot));
}
}
}
}
}
protected override IEnumerator Capture(bool anchor)
{
yield return(new WaitForSeconds(0.25f));
m_bCaptureRunning = true;
float captureStartTime = Time.realtimeSinceStartup;
float uploadStartTime = Time.realtimeSinceStartup;
ARCameraImageBytes image = null;
bool isHD = HWARHelper.TryGetCameraImageBytes(out image);
if (image != null && image.IsAvailable)
{
CoroutineJobCapture j = new CoroutineJobCapture();
j.host = this;
j.run = (int)(m_ImageRun & 0xEFFFFFFF);
j.index = m_ImageIndex++;
j.anchor = anchor;
if (gpsOn)
{
j.latitude = m_Latitude;
j.longitude = m_Longitude;
j.altitude = m_Altitude;
}
else
{
j.latitude = j.longitude = j.altitude = 0.0;
}
Camera cam = this.mainCamera;
Quaternion _q = cam.transform.rotation;
Matrix4x4 r = Matrix4x4.Rotate(new Quaternion(_q.x, _q.y, -_q.z, -_q.w));
Vector3 _p = cam.transform.position;
Vector3 p = new Vector3(_p.x, _p.y, -_p.z);
j.rotation = r;
j.position = p;
j.intrinsics = isHD ? HWARHelper.GetIntrinsics() : HWARHelper.GetIntrinsics(image.Width, image.Height);
int width = image.Width;
int height = image.Height;
byte[] pixels;
int channels = 0;
HWARHelper.GetPlaneData(out pixels, image);
channels = 1;
byte[] capture = new byte[channels * width * height + 1024];
Task <(string, icvCaptureInfo)> t = Task.Run(() =>
{
icvCaptureInfo info = Core.CaptureImage(capture, capture.Length, pixels, width, height, channels);
return(Convert.ToBase64String(capture, 0, info.captureSize), info);
});
while (!t.IsCompleted)
{
yield return(null);
}
j.encodedImage = t.Result.Item1;
NotifyIfConnected(t.Result.Item2);
if (m_SessionFirstImage)
{
m_SessionFirstImage = false;
}
j.OnStart += () =>
{
uploadStartTime = Time.realtimeSinceStartup;
mappingUIManager.SetProgress(0);
mappingUIManager.ShowProgressBar();
};
j.OnSuccess += (SDKImageResult result) =>
{
if (result.error == "none")
{
float et = Time.realtimeSinceStartup - uploadStartTime;
Debug.Log(string.Format("Image uploaded successfully in {0} seconds", et));
}
mappingUIManager.HideProgressBar();
};
j.OnProgress += (float progress) =>
{
int value = (int)(100f * progress);
Debug.Log(string.Format("Upload progress: {0}%", value));
mappingUIManager.SetProgress(value);
};
m_Jobs.Add(j);
image.Dispose();
float elapsedTime = Time.realtimeSinceStartup - captureStartTime;
Debug.Log(string.Format("Capture in {0} seconds", elapsedTime));
}
m_bCaptureRunning = false;
var captureButton = workspaceManager.captureButton.GetComponent <Button>();
captureButton.interactable = true;
}
public override IEnumerator RunJob()
{
Debug.Log("*************************** CoroutineJobCapture ***************************");
BaseMapper mapper = host as BaseMapper;
byte[] capture = new byte[channels * width * height + 1024];
Task <(string, icvCaptureInfo)> t = Task.Run(() =>
{
icvCaptureInfo info = Core.CaptureImage(capture, capture.Length, pixels, width, height, channels);
return(Convert.ToBase64String(capture, 0, info.captureSize), info);
});
while (!t.IsCompleted)
{
yield return(null);
}
string encodedImage = t.Result.Item1;
icvCaptureInfo captureInfo = t.Result.Item2;
mapper.NotifyIfConnected(captureInfo);
SDKImageRequest imageRequest = new SDKImageRequest();
imageRequest.token = mapper.token;
imageRequest.run = this.run;
imageRequest.bank = mapper.currentBank;
imageRequest.index = this.index;
imageRequest.anchor = this.anchor;
imageRequest.px = position.x;
imageRequest.py = position.y;
imageRequest.pz = position.z;
imageRequest.r00 = rotation.m00;
imageRequest.r01 = rotation.m01;
imageRequest.r02 = rotation.m02;
imageRequest.r10 = rotation.m10;
imageRequest.r11 = rotation.m11;
imageRequest.r12 = rotation.m12;
imageRequest.r20 = rotation.m20;
imageRequest.r21 = rotation.m21;
imageRequest.r22 = rotation.m22;
imageRequest.fx = intrinsics.x;
imageRequest.fy = intrinsics.y;
imageRequest.ox = intrinsics.z;
imageRequest.oy = intrinsics.w;
imageRequest.latitude = latitude;
imageRequest.longitude = longitude;
imageRequest.altitude = altitude;
imageRequest.b64 = encodedImage;
string jsonString = JsonUtility.ToJson(imageRequest);
using (UnityWebRequest request = UnityWebRequest.Put(string.Format(Endpoint.URL_FORMAT, mapper.server, Endpoint.CAPTURE_IMAGE), jsonString))
{
request.method = UnityWebRequest.kHttpVerbPOST;
request.useHttpContinue = false;
request.SetRequestHeader("Content-Type", "application/json");
request.SetRequestHeader("Accept", "application/json");
yield return(request.SendWebRequest());
Debug.Log("Response code: " + request.responseCode);
if (request.isNetworkError || request.isHttpError)
{
Debug.LogError(request.error);
}
else if (request.responseCode == (long)HttpStatusCode.OK)
{
SDKImageResult result = JsonUtility.FromJson <SDKImageResult>(request.downloadHandler.text);
if (result.error == "none")
{
Debug.Log("Image uploaded successfully");
}
}
}
}
public override IEnumerator RunJob()
{
Debug.Log("*************************** CoroutineJobLocalize On-Server ***************************");
byte[] capture = new byte[width * height + 1024];
Task <(string, icvCaptureInfo)> t = Task.Run(() =>
{
icvCaptureInfo info = Immersal.Core.CaptureImage(capture, capture.Length, pixels, width, height, 1);
return(Convert.ToBase64String(capture, 0, info.captureSize), info);
});
while (!t.IsCompleted)
{
yield return(null);
}
string encodedImage = t.Result.Item1;
icvCaptureInfo captureInfo = t.Result.Item2;
SDKLocalizeRequest imageRequest = new SDKLocalizeRequest();
imageRequest.token = this.token;
imageRequest.fx = intrinsics.x;
imageRequest.fy = intrinsics.y;
imageRequest.ox = intrinsics.z;
imageRequest.oy = intrinsics.w;
imageRequest.b64 = encodedImage;
int n = pcr.Count;
imageRequest.mapIds = new SDKMapId[n];
int count = 0;
foreach (int id in pcr.Keys)
{
imageRequest.mapIds[count] = new SDKMapId();
imageRequest.mapIds[count++].id = id;
}
string jsonString = JsonUtility.ToJson(imageRequest);
using (UnityWebRequest request = UnityWebRequest.Put(string.Format(Endpoint.URL_FORMAT, this.server, Endpoint.SERVER_LOCALIZE), jsonString))
{
request.method = UnityWebRequest.kHttpVerbPOST;
request.useHttpContinue = false;
request.SetRequestHeader("Content-Type", "application/json");
request.SetRequestHeader("Accept", "application/json");
yield return(request.SendWebRequest());
if (request.isNetworkError || request.isHttpError)
{
Debug.LogError(request.error);
}
else if (request.responseCode == (long)HttpStatusCode.OK)
{
SDKLocalizeResult result = JsonUtility.FromJson <SDKLocalizeResult>(request.downloadHandler.text);
if (result.success)
{
Matrix4x4 cloudSpace = Matrix4x4.identity;
cloudSpace.m00 = result.r00; cloudSpace.m01 = result.r01; cloudSpace.m02 = result.r02; cloudSpace.m03 = result.px;
cloudSpace.m10 = result.r10; cloudSpace.m11 = result.r11; cloudSpace.m12 = result.r12; cloudSpace.m13 = result.py;
cloudSpace.m20 = result.r20; cloudSpace.m21 = result.r21; cloudSpace.m22 = result.r22; cloudSpace.m23 = result.pz;
Matrix4x4 trackerSpace = Matrix4x4.TRS(position, rotation, Vector3.one);
stats.locSucc++;
Debug.Log("*************************** On-Server Localization Succeeded ***************************");
Debug.Log("fc 4x4\n" + cloudSpace + "\n" +
"ft 4x4\n" + trackerSpace);
Matrix4x4 m = trackerSpace * (cloudSpace.inverse);
foreach (KeyValuePair <int, PointCloudRenderer> p in pcr)
{
if (p.Key == result.map)
{
p.Value.go.transform.position = m.GetColumn(3);
p.Value.go.transform.rotation = m.rotation;
break;
}
}
Debug.Log(result.error);
}
else
{
stats.locFail++;
Debug.Log("*************************** On-Server Localization Failed ***************************");
}
}
}
}
public async void LocalizeGeoPose(SDKMapId[] mapIds)
{
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
#if PLATFORM_LUMIN
XRCameraImage image;
if (cameraSubsystem.TryGetLatestImage(out image))
#else
XRCpuImage image;
if (cameraSubsystem.TryAcquireLatestCpuImage(out image))
#endif
{
JobGeoPoseAsync j = new JobGeoPoseAsync();
byte[] pixels;
Camera cam = this.mainCamera;
Vector3 camPos = cam.transform.position;
Quaternion camRot = cam.transform.rotation;
int channels = 1;
int width = image.width;
int height = image.height;
j.mapIds = mapIds;
j.param1 = mapperSettings.param1;
j.param2 = mapperSettings.param2;
j.param3 = mapperSettings.param3;
j.param4 = mapperSettings.param4;
ARHelper.GetIntrinsics(out j.intrinsics);
ARHelper.GetPlaneData(out pixels, image);
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.OnResult += (SDKGeoPoseResult result) =>
{
if (result.success)
{
Debug.Log("*************************** GeoPose Localization Succeeded ***************************");
this.stats.locSucc++;
double latitude = result.latitude;
double longitude = result.longitude;
double ellipsoidHeight = result.ellipsoidHeight;
Quaternion rot = new Quaternion(result.quaternion[1], result.quaternion[2], result.quaternion[3], result.quaternion[0]);
Debug.Log(string.Format("GeoPose returned latitude: {0}, longitude: {1}, ellipsoidHeight: {2}, quaternion: {3}", latitude, longitude, ellipsoidHeight, rot));
double[] ecef = new double[3];
double[] wgs84 = new double[3] {
latitude, longitude, ellipsoidHeight
};
Core.PosWgs84ToEcef(ecef, wgs84);
JobEcefAsync je = new JobEcefAsync();
int mapId = mapIds[0].id;
je.id = mapId;
je.OnResult += (SDKEcefResult result2) =>
{
double[] mapToEcef = result2.ecef;
Vector3 mapPos;
Quaternion mapRot;
Core.PosEcefToMap(out mapPos, ecef, mapToEcef);
Core.RotEcefToMap(out mapRot, rot, mapToEcef);
Matrix4x4 cloudSpace = Matrix4x4.TRS(mapPos, mapRot, Vector3.one);
Matrix4x4 trackerSpace = Matrix4x4.TRS(camPos, camRot, Vector3.one);
Matrix4x4 m = trackerSpace * (cloudSpace.inverse);
foreach (KeyValuePair <int, PointCloudRenderer> p in this.pcr)
{
if (p.Key == mapId)
{
p.Value.go.transform.position = m.GetColumn(3);
p.Value.go.transform.rotation = m.rotation;
break;
}
}
LocalizerPose lastLocalizedPose;
LocalizerBase.GetLocalizerPose(out lastLocalizedPose, mapId, cloudSpace.GetColumn(3), cloudSpace.rotation, m.inverse, mapToEcef);
this.lastLocalizedPose = lastLocalizedPose;
};
m_Jobs.Add(je.RunJobAsync());
}
else
{
this.stats.locFail++;
Debug.Log("*************************** GeoPose Localization Failed ***************************");
}
};
m_Jobs.Add(j.RunJobAsync());
image.Dispose();
}
}
public override async void LocalizeServer()
{
bool rgb = false;
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
#if PLATFORM_LUMIN
XRCameraImage image;
if (cameraSubsystem.TryGetLatestImage(out image))
#else
XRCpuImage image;
if (cameraSubsystem.TryAcquireLatestCpuImage(out image))
#endif
{
JobLocalizeServerAsync j = new JobLocalizeServerAsync();
if (mapperSettings.serverLocalizationWithIds)
{
int n = pcr.Count;
j.mapIds = new SDKMapId[n];
int count = 0;
foreach (int id in pcr.Keys)
{
j.mapIds[count] = new SDKMapId();
j.mapIds[count++].id = id;
}
}
else
{
j.useGPS = true;
j.latitude = m_Latitude;
j.longitude = m_Longitude;
j.radius = DefaultRadius;
}
byte[] pixels;
Camera cam = this.mainCamera;
Vector3 camPos = cam.transform.position;
Quaternion camRot = cam.transform.rotation;
int channels = 1;
int width = image.width;
int height = image.height;
j.rotation = camRot;
j.position = camPos;
j.param1 = mapperSettings.param1;
j.param2 = mapperSettings.param2;
j.param3 = mapperSettings.param3;
j.param4 = mapperSettings.param4;
ARHelper.GetIntrinsics(out j.intrinsics);
if (rgb)
{
ARHelper.GetPlaneDataRGB(out pixels, image);
channels = 3;
}
else
{
ARHelper.GetPlaneData(out pixels, image);
}
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.OnResult += (SDKLocalizeResult result) =>
{
if (result.success)
{
Matrix4x4 m = Matrix4x4.identity;
Matrix4x4 cloudSpace = Matrix4x4.identity;
cloudSpace.m00 = result.r00; cloudSpace.m01 = result.r01; cloudSpace.m02 = result.r02; cloudSpace.m03 = result.px;
cloudSpace.m10 = result.r10; cloudSpace.m11 = result.r11; cloudSpace.m12 = result.r12; cloudSpace.m13 = result.py;
cloudSpace.m20 = result.r20; cloudSpace.m21 = result.r21; cloudSpace.m22 = result.r22; cloudSpace.m23 = result.pz;
Matrix4x4 trackerSpace = Matrix4x4.TRS(camPos, camRot, Vector3.one);
this.stats.locSucc++;
Debug.Log("*************************** On-Server Localization Succeeded ***************************");
Debug.Log(string.Format("params: {0}, {1}, {2}, {3}", j.param1, j.param2, j.param3, j.param4));
Debug.Log("fc 4x4\n" + cloudSpace + "\n" +
"ft 4x4\n" + trackerSpace);
m = trackerSpace * (cloudSpace.inverse);
foreach (KeyValuePair <int, PointCloudRenderer> p in this.pcr)
{
if (p.Key == result.map)
{
p.Value.go.transform.position = m.GetColumn(3);
p.Value.go.transform.rotation = m.rotation;
break;
}
}
JobEcefAsync je = new JobEcefAsync();
je.id = result.map;
je.OnResult += (SDKEcefResult result2) =>
{
LocalizerPose lastLocalizedPose;
LocalizerBase.GetLocalizerPose(out lastLocalizedPose, result.map, cloudSpace.GetColumn(3), cloudSpace.rotation, m.inverse, result2.ecef);
this.lastLocalizedPose = lastLocalizedPose;
};
m_Jobs.Add(je.RunJobAsync());
}
else
{
this.stats.locFail++;
Debug.Log("*************************** On-Server Localization Failed ***************************");
}
};
m_Jobs.Add(j.RunJobAsync());
image.Dispose();
}
}
public override IEnumerator RunJob()
{
Debug.Log("*************************** CoroutineJobCapture ***************************");
byte[] capture = new byte[4 * 1024 * 1024]; // should need less than 2 megs when reso is 1440p
Task <(string, icvCaptureInfo)> t = Task.Run(() =>
{
icvCaptureInfo info = Core.CaptureImage(capture, capture.Length, pixels, width, height, channels);
return(Convert.ToBase64String(capture, 0, info.captureSize), info);
});
while (!t.IsCompleted)
{
yield return(null);
}
string encodedImage = t.Result.Item1;
icvCaptureInfo captureInfo = t.Result.Item2;
if (!sessionFirstImage)
{
if (captureInfo.connected == 0)
{
onFailedToConnect.Invoke();
}
else
{
onConnect.Invoke();
}
}
SDKImageRequest imageRequest = new SDKImageRequest();
imageRequest.token = this.token;
imageRequest.run = this.run;
imageRequest.bank = this.bank;
imageRequest.index = this.index;
imageRequest.anchor = this.anchor;
imageRequest.px = position.x;
imageRequest.py = position.y;
imageRequest.pz = position.z;
imageRequest.r00 = rotation.m00;
imageRequest.r01 = rotation.m01;
imageRequest.r02 = rotation.m02;
imageRequest.r10 = rotation.m10;
imageRequest.r11 = rotation.m11;
imageRequest.r12 = rotation.m12;
imageRequest.r20 = rotation.m20;
imageRequest.r21 = rotation.m21;
imageRequest.r22 = rotation.m22;
imageRequest.fx = intrinsics.x;
imageRequest.fy = intrinsics.y;
imageRequest.ox = intrinsics.z;
imageRequest.oy = intrinsics.w;
imageRequest.b64 = encodedImage;
string jsonString = JsonUtility.ToJson(imageRequest);
using (UnityWebRequest request = UnityWebRequest.Put(string.Format(URL_FORMAT, this.server, "1"), jsonString))
{
request.method = UnityWebRequest.kHttpVerbPOST;
request.useHttpContinue = false;
request.SetRequestHeader("Content-Type", "application/json");
request.SetRequestHeader("Accept", "application/json");
yield return(request.SendWebRequest());
Debug.Log("Response code: " + request.responseCode);
if (request.isNetworkError || request.isHttpError)
{
Debug.Log(request.error);
}
else if (request.responseCode == (long)HttpStatusCode.OK)
{
Debug.Log(request.downloadHandler.text);
}
}
}
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);
}
