public void LocalizeServer()
{
ARCameraManager cameraManager = sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
XRCameraImage image;
if (cameraSubsystem.TryGetLatestImage(out image))
{
CoroutineJobLocalizeServer j = new CoroutineJobLocalizeServer();
j.server = this.server;
j.token = this.token;
Camera cam = Camera.main;
j.rotation = cam.transform.rotation;
j.position = cam.transform.position;
j.intrinsics = ARHelper.GetIntrinsics(cameraManager);
j.width = image.width;
j.height = image.height;
j.stats = this.stats;
j.pcr = this.pcr;
ARHelper.GetPlaneData(out j.pixels, image);
jobs.Add(j);
image.Dispose();
}
}
void Update()
{
if (!isLocalPlayer)
{
return;
}
#if UNITY_EDITOR || UNITY_STANDALONE
var x = Input.GetAxis("Horizontal") * Time.deltaTime * 150.0f;
var z = Input.GetAxis("Vertical") * Time.deltaTime * 3.0f;
Vector3 oldPos = transform.position;
Quaternion oldRot = transform.rotation;
transform.Rotate(0, x, 0);
transform.Translate(0, 0, z);
m_Position = transform.position;
m_Rotation = transform.rotation;
if (m_Position != oldPos || m_Rotation != oldRot)
{
m_MainCamera.transform.position = m_Position;
m_MainCamera.transform.rotation = m_Rotation;
if (isServer)
{
UpdateClientsPos();
}
else
{
CmdMove(m_Position, m_Rotation);
}
}
#elif UNITY_IOS || UNITY_ANDROID
Vector3 oldPos = transform.localPosition;
Quaternion oldRot = transform.localRotation;
Vector3 camPos = m_MainCamera.transform.position;
Quaternion camRot = m_MainCamera.transform.rotation;
camPos += m_MainCamera.transform.up * .2f;
Matrix4x4 cloudSpace = ARHelper.ToCloudSpace(camPos, camRot, m_Loc.position, m_Loc.rotation);
m_Rotation = cloudSpace.rotation;
m_Position = cloudSpace.GetColumn(3);
if (m_Position != oldPos || m_Rotation != oldRot)
{
transform.localPosition = m_Position;
transform.localRotation = m_Rotation;
if (isServer)
{
UpdateClientsPos();
}
else
{
CmdMove(m_Position, m_Rotation);
}
}
#endif
}
public void Create()
{
try {
ARHelper.CreateSchema(Context);
Context.SuccessMessage("Schema has been created successfully.");
} catch (System.Exception e) {
ErrorMessages(e);
}
RedirectToAction("index");
}
public void CreateScript()
{
try {
ARHelper.GenerateCreationScripts(Context.Server.MapPath("~/schema.sql"));
Context.SuccessMessage("Schema script has been created successfully.");
} catch (System.Exception e) {
ErrorMessages(e);
}
RedirectToAction("index");
}
public void Drop()
{
try {
ARHelper.DropSchema();
Context.SuccessMessage("Schema has been dropped successfully.");
} catch (System.Exception e) {
ErrorMessages(e);
}
RedirectToAction("index");
}
public override void Localize()
{
XRCpuImage image;
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
if (cameraSubsystem != null && cameraSubsystem.TryAcquireLatestCpuImage(out image))
{
CoroutineJobLocalize j = new CoroutineJobLocalize();
Camera cam = this.mainCamera;
Vector3 camPos = cam.transform.position;
Quaternion camRot = cam.transform.rotation;
j.intrinsics = ARHelper.GetIntrinsics();
j.width = image.width;
j.height = image.height;
j.rotation = camRot;
j.position = camPos;
j.OnSuccess += (int mapId, Vector3 position, Quaternion rotation) =>
{
this.stats.locSucc++;
Debug.Log("*************************** Localization Succeeded ***************************");
Matrix4x4 cloudSpace = Matrix4x4.TRS(position, rotation, Vector3.one);
Matrix4x4 trackerSpace = Matrix4x4.TRS(camPos, camRot, Vector3.one);
Debug.Log("id " + mapId + "\n" +
"fc 4x4\n" + cloudSpace + "\n" +
"ft 4x4\n" + trackerSpace);
Matrix4x4 m = trackerSpace * (cloudSpace.inverse);
LocalizerPose lastLocalizedPose;
LocalizerBase.GetLocalizerPose(out lastLocalizedPose, mapId, position, rotation, m.inverse);
this.lastLocalizedPose = lastLocalizedPose;
foreach (PointCloudRenderer p in this.pcr.Values)
{
if (p.mapId == mapId)
{
p.go.transform.position = m.GetColumn(3);
p.go.transform.rotation = m.rotation;
break;
}
}
};
j.OnFail += () =>
{
this.stats.locFail++;
Debug.Log("*************************** Localization Failed ***************************");
};
ARHelper.GetPlaneData(out j.pixels, image);
m_Jobs.Add(j);
image.Dispose();
}
}
private IEnumerator Capture(bool anchor)
{
yield return(new WaitForSeconds(0.25f));
XRCameraImage image;
ARCameraManager cameraManager = sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
if (cameraSubsystem != null && cameraSubsystem.TryGetLatestImage(out image))
{
CoroutineJobCapture j = new CoroutineJobCapture();
j.onConnect = onConnect;
j.onFailedToConnect = onFailedToConnect;
j.server = this.server;
j.token = this.token;
j.bank = this.bank;
j.run = (int)(this.imageRun & 0xEFFFFFFF);
j.index = this.imageIndex++;
j.anchor = anchor;
Camera cam = Camera.main;
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 = ARHelper.GetIntrinsics(cameraManager);
j.width = image.width;
j.height = image.height;
if (rgbCapture)
{
ARHelper.GetPlaneDataRGB(out j.pixels, image);
j.channels = 3;
}
else
{
ARHelper.GetPlaneData(out j.pixels, image);
j.channels = 1;
}
j.sessionFirstImage = sessionFirstImage;
if (sessionFirstImage)
{
sessionFirstImage = false;
}
jobs.Add(j);
image.Dispose();
m_cameraShutterClick.Play();
}
}
private void Update()
{
bool trackingQualityAvailable = false;
if (ImmersalSDK.isHWAR)
{
#if HWAR
trackingQualityAvailable = HWARHelper.TryGetTrackingQuality(out m_SLAMTrackingQuality);
#endif
}
else
{
trackingQualityAvailable = ARHelper.TryGetTrackingQuality(out m_SLAMTrackingQuality);
}
if (trackingQualityAvailable && this.Localizer != null)
{
LocalizerStats stats = this.Localizer.stats;
if (stats.localizationAttemptCount > 0)
{
int q = CurrentResults(stats.localizationSuccessCount);
if (q > m_SLAMTrackingQuality)
{
q = m_SLAMTrackingQuality;
}
if (!m_HasPose && q > 1)
{
m_HasPose = true;
onPoseFound?.Invoke();
}
if (m_HasPose && q < 1 && m_SLAMTrackingQuality == 0)
{
m_HasPose = false;
onPoseLost?.Invoke();
}
this.TrackingQuality = q;
}
}
if (!isHWAR)
{
if (!m_bCamConfigDone && cameraManager != null)
{
ConfigureCamera();
}
}
}
private async Task MapAlignmentSave(int mapId, Matrix4x4 m)
{
//
// Updates map metadata to the Cloud Service and reloads to keep local files in sync
//
Vector3 pos = m.GetColumn(3);
Quaternion rot = m.rotation;
float scl = (m.lossyScale.x + m.lossyScale.y + m.lossyScale.z) / 3f; // Only uniform scale metadata is supported
// IMPORTANT
// Switching coordinate system handedness from Unity's left-handed system to Immersal Cloud Service's default right-handed system
Matrix4x4 a;
Matrix4x4 b = Matrix4x4.TRS(pos, rot, transform.localScale);
ARHelper.SwitchHandedness(out a, b);
pos = a.GetColumn(3);
rot = a.rotation;
// Update map alignment metadata to Immersal Cloud Service
JobMapAlignmentSetAsync j = new JobMapAlignmentSetAsync();
j.id = mapId;
j.tx = pos.x;
j.ty = pos.y;
j.tz = pos.z;
j.qx = rot.x;
j.qy = rot.y;
j.qz = rot.z;
j.qw = rot.w;
j.scale = scl;
j.OnResult += (SDKMapAlignmentSetResult result) =>
{
Debug.Log(string.Format("Alignment for map {0} saved", mapId));
};
j.OnError += (e) =>
{
Immersal.Samples.Mapping.NotificationManager.Instance.GenerateError("Network Error");
Debug.Log(string.Format("Failed to save alignment for map id {0}\n{1}", mapId, e));
};
await j.RunJobAsync();
}
public override void LocalizeServer()
{
bool rgb = false;
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
XRCameraImage image;
if (cameraSubsystem.TryGetLatestImage(out image))
{
CoroutineJobLocalizeServer j = new CoroutineJobLocalizeServer();
j.host = this;
if (this.useGPS)
{
j.useGPS = true;
j.latitude = m_Latitude;
j.longitude = m_Longitude;
j.radius = DefaultRadius;
}
Camera cam = this.mainCamera;
j.rotation = cam.transform.rotation;
j.position = cam.transform.position;
j.intrinsics = ARHelper.GetIntrinsics();
j.width = image.width;
j.height = image.height;
if (rgb)
{
ARHelper.GetPlaneDataRGB(out j.pixels, image);
j.channels = 3;
}
else
{
ARHelper.GetPlaneData(out j.pixels, image);
j.channels = 1;
}
m_Jobs.Add(j);
image.Dispose();
}
}
public override void Localize()
{
XRCameraImage image;
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
if (cameraSubsystem != null && cameraSubsystem.TryGetLatestImage(out image))
{
CoroutineJobLocalize j = new CoroutineJobLocalize();
Camera cam = this.mainCamera;
j.intrinsics = ARHelper.GetIntrinsics();
j.width = image.width;
j.height = image.height;
j.rotation = cam.transform.rotation;
j.position = cam.transform.position;
j.host = this;
ARHelper.GetPlaneData(out j.pixels, image);
m_Jobs.Add(j);
image.Dispose();
}
}
public void Localize()
{
XRCameraImage image;
if (m_CameraManager.TryGetLatestImage(out image))
{
CoroutineJobLocalize j = new CoroutineJobLocalize();
Camera cam = Camera.main;
j.rotation = cam.transform.rotation;
j.position = cam.transform.position;
j.intrinsics = ARHelper.GetIntrinsics(m_CameraManager);
j.width = image.width;
j.height = image.height;
j.stats = this.stats;
j.pcr = this.pcr;
XRCameraImagePlane plane = image.GetPlane(0); // use the Y plane
j.pixels = new byte[plane.data.Length];
plane.data.CopyTo(j.pixels);
jobs.Add(j);
image.Dispose();
}
}
protected override IEnumerator Capture(bool anchor)
{
yield return(new WaitForSeconds(0.25f));
m_bCaptureRunning = true;
float captureStartTime = Time.realtimeSinceStartup;
float uploadStartTime = Time.realtimeSinceStartup;
XRCpuImage image;
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
if (cameraSubsystem != null && cameraSubsystem.TryAcquireLatestCpuImage(out image))
{
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 = ARHelper.GetIntrinsics();
int width = image.width;
int height = image.height;
byte[] pixels;
int channels = 0;
if (m_RgbCapture)
{
ARHelper.GetPlaneDataRGB(out pixels, image);
channels = 3;
}
else
{
ARHelper.GetPlaneData(out pixels, image);
channels = 1;
}
byte[] capture = new byte[channels * width * height + 1024];
Task <icvCaptureInfo> captureTask = Task.Run(() =>
{
return(Core.CaptureImage(capture, capture.Length, pixels, width, height, channels));
});
Task <string> convertTask = captureTask.ContinueWith <string>((antecedent) =>
{
return(Convert.ToBase64String(capture, 0, antecedent.Result.captureSize));
});
while (!convertTask.IsCompleted)
{
yield return(null);
}
j.encodedImage = convertTask.Result;
NotifyIfConnected(captureTask.Result);
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 async void SubmitAlignment()
{
if (mapperSettings == null)
{
Debug.Log("ActiveMapListControl: MapperSettings not found");
return;
}
bool transformRootToOrigin = mapperSettings.transformRootToOrigin;
// Debug.Log(string.Format("clicked, root id: {0}, map count {1}", rootMapId, m_ActiveMaps.Count));
if (rootMapId > 0 && m_ActiveMaps.Count > 1)
{
Transform root = ARSpace.mapIdToMap[rootMapId].transform;
Matrix4x4 worldSpaceRoot = root.localToWorldMatrix;
foreach (KeyValuePair <int, ARMap> entry in ARSpace.mapIdToMap)
{
if (entry.Value.mapId != rootMapId)
{
// Debug.Log(string.Format("looping... {0}", entry.Value.mapId));
Transform xf = entry.Value.transform;
Matrix4x4 worldSpaceTransform = xf.localToWorldMatrix;
if (transformRootToOrigin)
{
Matrix4x4 offset = worldSpaceRoot.inverse * worldSpaceTransform;
await MapAlignmentSave(entry.Value.mapId, offset);
}
else
{
// TODO: implement ECEF/double support
Vector3 rootPosMetadata = new Vector3((float)ARSpace.mapIdToMap[rootMapId].mapAlignment.tx, (float)ARSpace.mapIdToMap[rootMapId].mapAlignment.ty, (float)ARSpace.mapIdToMap[rootMapId].mapAlignment.tz);
Quaternion rootRotMetadata = new Quaternion((float)ARSpace.mapIdToMap[rootMapId].mapAlignment.qx, (float)ARSpace.mapIdToMap[rootMapId].mapAlignment.qy, (float)ARSpace.mapIdToMap[rootMapId].mapAlignment.qz, (float)ARSpace.mapIdToMap[rootMapId].mapAlignment.qw);
// IMPORTANT
// Switch coordinate system handedness back from Immersal Cloud Service's default right-handed system to Unity's left-handed system
Matrix4x4 a;
Matrix4x4 b = Matrix4x4.TRS(rootPosMetadata, rootRotMetadata, Vector3.one);
ARHelper.SwitchHandedness(out a, b);
Matrix4x4 offset = a * worldSpaceRoot.inverse * worldSpaceTransform;
await MapAlignmentSave(entry.Value.mapId, offset);
}
}
else
{
if (transformRootToOrigin)
{
// set root to origin
Matrix4x4 identity = Matrix4x4.identity;
await MapAlignmentSave(entry.Value.mapId, identity);
}
else
{
// or keep the root transform
// await MapAlignmentSave(entry.Value.mapId, worldSpaceRoot);
}
}
}
m_VisualizeManager?.DefaultView();
Immersal.Samples.Mapping.NotificationManager.Instance.GenerateSuccess("Map Alignments Saved");
}
}
protected override async void Capture(bool anchor)
{
await Task.Delay(250);
m_bCaptureRunning = true;
float captureStartTime = Time.realtimeSinceStartup;
float uploadStartTime = Time.realtimeSinceStartup;
#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
{
JobCaptureAsync j = new JobCaptureAsync();
j.run = (int)(m_ImageRun & 0xEFFFFFFF);
j.index = m_ImageIndex++;
j.anchor = anchor;
if (mapperSettings.useGps)
{
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 rot = Matrix4x4.Rotate(new Quaternion(_q.x, _q.y, -_q.z, -_q.w));
Vector3 _p = cam.transform.position;
Vector3 pos = new Vector3(_p.x, _p.y, -_p.z);
j.rotation = rot;
j.position = pos;
ARHelper.GetIntrinsics(out j.intrinsics);
int width = image.width;
int height = image.height;
byte[] pixels;
int channels = 1;
if (mapperSettings.captureRgb)
{
ARHelper.GetPlaneDataRGB(out pixels, image);
channels = 3;
}
else
{
ARHelper.GetPlaneData(out pixels, image);
}
byte[] capture = new byte[channels * width * height + 1024];
Task <icvCaptureInfo> captureTask = Task.Run(() =>
{
return(Core.CaptureImage(capture, capture.Length, pixels, width, height, channels));
});
await captureTask;
string path = string.Format("{0}/{1}", this.tempImagePath, System.Guid.NewGuid());
using (BinaryWriter writer = new BinaryWriter(File.OpenWrite(path)))
{
writer.Write(capture, 0, captureTask.Result.captureSize);
}
j.imagePath = path;
j.encodedImage = "";
NotifyIfConnected(captureTask.Result);
if (m_SessionFirstImage)
{
m_SessionFirstImage = false;
}
j.OnStart += () =>
{
uploadStartTime = Time.realtimeSinceStartup;
mappingUIManager.SetProgress(0);
mappingUIManager.ShowProgressBar();
};
j.OnResult += (SDKImageResult result) =>
{
float et = Time.realtimeSinceStartup - uploadStartTime;
Debug.Log(string.Format("Image uploaded successfully in {0} seconds", et));
mappingUIManager.HideProgressBar();
};
j.Progress.ProgressChanged += (s, progress) =>
{
int value = (int)(100f * progress);
//Debug.Log(string.Format("Upload progress: {0}%", value));
mappingUIManager.SetProgress(value);
};
j.OnError += (e) =>
{
mappingUIManager.HideProgressBar();
};
m_Jobs.Add(j.RunJobAsync());
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 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 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();
}
}
public override void LocalizeServer()
{
bool rgb = false;
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
XRCpuImage image;
if (cameraSubsystem.TryAcquireLatestCpuImage(out image))
{
CoroutineJobLocalizeServer j = new CoroutineJobLocalizeServer();
if (gpsOn)
{
j.useGPS = true;
j.latitude = m_Latitude;
j.longitude = m_Longitude;
j.radius = DefaultRadius;
}
else
{
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;
}
}
Camera cam = this.mainCamera;
Vector3 camPos = cam.transform.position;
Quaternion camRot = cam.transform.rotation;
j.host = this;
j.rotation = camRot;
j.position = camPos;
j.intrinsics = ARHelper.GetIntrinsics();
j.width = image.width;
j.height = image.height;
if (rgb)
{
ARHelper.GetPlaneDataRGB(out j.pixels, image);
j.channels = 3;
}
else
{
ARHelper.GetPlaneData(out j.pixels, image);
j.channels = 1;
}
j.OnResult += (SDKLocalizeResult result) =>
{
/*if (result.error == "none")
* {*/
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("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;
}
}
CoroutineJobEcef je = new CoroutineJobEcef();
je.host = this;
je.id = result.map;
je.OnSuccess += (SDKEcefResult result2) =>
{
if (result2.error == "none")
{
Debug.Log(result2.ecef);
LocalizerPose lastLocalizedPose;
LocalizerBase.GetLocalizerPose(out lastLocalizedPose, result.map, cloudSpace.GetColumn(3), cloudSpace.rotation, m.inverse, result2.ecef);
this.lastLocalizedPose = lastLocalizedPose;
}
else
{
Debug.LogError(result2.error);
}
};
m_Jobs.Add(je);
}
else
{
this.stats.locFail++;
Debug.Log("*************************** On-Server Localization Failed ***************************");
}
/*}
* else
* {
* Debug.LogError(result.error);
* }*/
};
m_Jobs.Add(j);
image.Dispose();
}
}
protected override IEnumerator Capture(bool anchor)
{
yield return(new WaitForSeconds(0.25f));
m_bCaptureRunning = true;
XRCameraImage image;
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
if (cameraSubsystem != null && cameraSubsystem.TryGetLatestImage(out image))
{
CoroutineJobCapture j = new CoroutineJobCapture();
j.host = this;
j.run = (int)(m_ImageRun & 0xEFFFFFFF);
j.index = m_ImageIndex++;
j.anchor = anchor;
if (useGPS)
{
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 = ARHelper.GetIntrinsics();
j.width = image.width;
j.height = image.height;
if (m_RgbCapture)
{
ARHelper.GetPlaneDataRGB(out j.pixels, image);
j.channels = 3;
}
else
{
ARHelper.GetPlaneData(out j.pixels, image);
j.channels = 1;
}
if (m_SessionFirstImage)
{
m_SessionFirstImage = false;
}
m_Jobs.Add(j);
image.Dispose();
}
m_bCaptureRunning = false;
var captureButton = workspaceManager.captureButton.GetComponent <Button>();
captureButton.interactable = true;
}
virtual public async void Capture()
{
await Task.Delay(250);
m_bCaptureRunning = true;
float captureStartTime = Time.realtimeSinceStartup;
float uploadStartTime = Time.realtimeSinceStartup;
XRCpuImage image;
ARCameraManager cameraManager = m_Sdk.cameraManager;
var cameraSubsystem = cameraManager.subsystem;
if (cameraSubsystem != null && cameraSubsystem.TryAcquireLatestCpuImage(out image))
{
JobCaptureAsync j = new JobCaptureAsync();
j.run = (int)(m_ImageRun & 0xEFFFFFFF);
j.index = m_ImageIndex++;
j.anchor = false;
if (AutomaticCaptureLocationProvider.Instance.gpsOn)
{
j.latitude = AutomaticCaptureLocationProvider.Instance.latitude;
j.longitude = AutomaticCaptureLocationProvider.Instance.longitude;
j.altitude = AutomaticCaptureLocationProvider.Instance.altitude;
}
else
{
j.latitude = j.longitude = j.altitude = 0.0;
}
ARHelper.GetIntrinsics(out j.intrinsics);
Quaternion rot = m_MainCamera.transform.rotation;
Vector3 pos = m_MainCamera.transform.position;
ARHelper.GetRotation(ref rot);
j.rotation = ARHelper.SwitchHandedness(Matrix4x4.Rotate(rot));
j.position = ARHelper.SwitchHandedness(pos);
int width = image.width;
int height = image.height;
byte[] pixels;
int channels = 1;
if (m_RgbCapture)
{
ARHelper.GetPlaneDataRGB(out pixels, image);
channels = 3;
}
else
{
ARHelper.GetPlaneData(out pixels, image);
}
byte[] capture = new byte[channels * width * height + 1024];
Task <icvCaptureInfo> captureTask = Task.Run(() =>
{
return(Core.CaptureImage(capture, capture.Length, pixels, width, height, channels));
});
await captureTask;
string path = string.Format("{0}/{1}", tempImagePath, System.Guid.NewGuid());
using (BinaryWriter writer = new BinaryWriter(File.OpenWrite(path)))
{
writer.Write(capture, 0, captureTask.Result.captureSize);
}
j.imagePath = path;
j.encodedImage = "";
j.OnStart += () =>
{
uploadStartTime = Time.realtimeSinceStartup;
};
j.OnResult += (SDKImageResult result) =>
{
float et = Time.realtimeSinceStartup - uploadStartTime;
Debug.Log(string.Format("Image uploaded successfully in {0} seconds", et));
OnImageUploaded?.Invoke();
};
j.Progress.ProgressChanged += (s, progress) =>
{
int value = (int)(100f * progress);
Debug.Log(string.Format("Upload progress: {0}%", value));
};
j.OnError += (e) =>
{
Debug.Log(string.Format("Capture error: " + e));
};
m_Jobs.Add(j);
image.Dispose();
float elapsedTime = Time.realtimeSinceStartup - captureStartTime;
Debug.Log(string.Format("Capture in {0} seconds", elapsedTime));
}
m_bCaptureRunning = false;
}
private IEnumerator Capture(bool anchor)
{
yield return(new WaitForSeconds(0.25f));
XRCameraImage image;
if (m_CameraManager.TryGetLatestImage(out image))
{
CoroutineJobCapture j = new CoroutineJobCapture();
j.onConnect = onConnect;
j.onFailedToConnect = onFailedToConnect;
j.server = this.server;
j.token = this.token;
j.bank = this.bank;
j.run = (int)(this.imageRun & 0xEFFFFFFF);
j.index = this.imageIndex++;
j.anchor = anchor;
Camera cam = Camera.main;
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 = ARHelper.GetIntrinsics(m_CameraManager);
j.width = image.width;
j.height = image.height;
if (rgbCapture)
{
var conversionParams = new XRCameraImageConversionParams
{
inputRect = new RectInt(0, 0, image.width, image.height),
outputDimensions = new Vector2Int(image.width, image.height),
outputFormat = TextureFormat.RGB24,
transformation = CameraImageTransformation.None
};
int size = image.GetConvertedDataSize(conversionParams);
j.pixels = new byte[size];
j.channels = 3;
GCHandle bufferHandle = GCHandle.Alloc(j.pixels, GCHandleType.Pinned);
image.Convert(conversionParams, bufferHandle.AddrOfPinnedObject(), j.pixels.Length);
bufferHandle.Free();
}
else
{
XRCameraImagePlane plane = image.GetPlane(0); // use the Y plane
j.pixels = new byte[plane.data.Length];
j.channels = 1;
plane.data.CopyTo(j.pixels);
}
j.sessionFirstImage = sessionFirstImage;
if (sessionFirstImage)
{
sessionFirstImage = false;
}
jobs.Add(j);
image.Dispose();
m_cameraShutterClick.Play();
}
}
