public static bool TryGetCameraImageBytes(out ARCameraImageBytes image)
{
bool isHD = false;
if (ImmersalSDK.Instance.androidResolution == ImmersalSDK.CameraResolution.Max)
{
try
{
image = ARFrame.AcquirPreviewImageBytes();
isHD = true;
}
catch (SystemException e)
{
Debug.LogError("Cannot acquire FullHD image: " + e.Message);
image = ARFrame.AcquireCameraImageBytes();
}
}
else
{
image = ARFrame.AcquireCameraImageBytes();
}
return(isHD);
}
public static void GetPlaneData(out byte[] pixels, ARCameraImageBytes image)
{
IntPtr plane = image.Y;
int width = image.Width, height = image.Height;
int size = width * height;
pixels = new byte[size];
if (width == image.YRowStride)
{
Marshal.Copy(plane, pixels, 0, size);
}
else
{
unsafe
{
ulong handle;
byte *srcPtr = (byte *)plane.ToPointer();
byte *dstPtr = (byte *)UnsafeUtility.PinGCArrayAndGetDataAddress(pixels, out handle);
if (width > 0 && height > 0)
{
UnsafeUtility.MemCpyStride(dstPtr, width, srcPtr, image.YRowStride, width, height);
}
UnsafeUtility.ReleaseGCObject(handle);
}
}
}
public override void Localize()
{
ARCameraImageBytes image = null;
bool isHD = HWARHelper.TryGetCameraImageBytes(out image);
if (image != null && image.IsAvailable)
{
CoroutineJobLocalize j = new CoroutineJobLocalize();
Camera cam = this.mainCamera;
Vector3 camPos = cam.transform.position;
Quaternion camRot = cam.transform.rotation;
j.intrinsics = isHD ? HWARHelper.GetIntrinsics() : HWARHelper.GetIntrinsics(image.Width, image.Height);
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 ***************************");
};
HWARHelper.GetPlaneData(out j.pixels, image);
m_Jobs.Add(j);
image.Dispose();
}
}
public override void LocalizeServer()
{
ARCameraImageBytes image = null;
if (m_Sdk.androidResolution == ImmersalSDK.CameraResolution.Max)
{
try
{
image = ARFrame.AcquirPreviewImageBytes();
}
catch (NullReferenceException e)
{
Debug.LogError("Cannot acquire FullHD image: " + e.Message);
image = ARFrame.AcquireCameraImageBytes();
}
}
else
{
image = ARFrame.AcquireCameraImageBytes();
}
if (image != null && image.IsAvailable)
{
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 = HWARHelper.GetIntrinsics();
j.width = image.Width;
j.height = image.Height;
HWARHelper.GetPlaneData(out j.pixels, image);
j.channels = 1;
m_Jobs.Add(j);
image.Dispose();
}
}
protected override IEnumerator Capture(bool anchor)
{
yield return(new WaitForSeconds(0.25f));
m_bCaptureRunning = true;
ARCameraImageBytes image = null;
if (m_Sdk.androidResolution == ImmersalSDK.CameraResolution.Max)
{
try
{
image = ARFrame.AcquirPreviewImageBytes();
}
catch (NullReferenceException e)
{
Debug.LogError("Cannot acquire FullHD image: " + e.Message);
image = ARFrame.AcquireCameraImageBytes();
}
}
else
{
image = ARFrame.AcquireCameraImageBytes();
}
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 (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 = HWARHelper.GetIntrinsics();
j.width = image.Width;
j.height = image.Height;
HWARHelper.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;
}
public override IEnumerator Localize()
{
ARCameraImageBytes image = null;
bool isHD = HWARHelper.TryGetCameraImageBytes(out image);
if (image != null && image.IsAvailable)
{
stats.localizationAttemptCount++;
Vector3 camPos = m_Cam.transform.position;
Quaternion camRot = m_Cam.transform.rotation;
byte[] pixels;
Vector4 intrinsics = isHD ? HWARHelper.GetIntrinsics() : HWARHelper.GetIntrinsics(image.Width, image.Height);
HWARHelper.GetPlaneData(out pixels, image);
image.Dispose();
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, pixels));
});
while (!t.IsCompleted)
{
yield return(null);
}
int mapId = t.Result;
if (mapId > 0 && ARSpace.mapIdToOffset.ContainsKey(mapId))
{
if (mapId != lastLocalizedMapId)
{
if (m_ResetOnMapChange)
{
foreach (KeyValuePair <Transform, SpaceContainer> item in ARSpace.transformToSpace)
{
item.Value.filter.ResetFiltering();
}
}
lastLocalizedMapId = mapId;
OnMapChanged?.Invoke(mapId);
}
HWARHelper.GetRotation(ref rot);
MapOffset mo = ARSpace.mapIdToOffset[mapId];
float elapsedTime = Time.realtimeSinceStartup - startTime;
Debug.Log(string.Format("Relocalised in {0} seconds", elapsedTime));
stats.localizationSuccessCount++;
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);
mo.space.filter.RefinePose(m);
LocalizerPose localizerPose;
GetLocalizerPose(out localizerPose, mapId, pos, rot, m.inverse);
OnPoseFound?.Invoke(localizerPose);
}
}
yield return(StartCoroutine(base.Localize()));
}
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 void LocalizeServer()
{
ARCameraImageBytes image = null;
bool isHD = HWARHelper.TryGetCameraImageBytes(out image);
if (image != null && image.IsAvailable)
{
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 = isHD ? HWARHelper.GetIntrinsics() : HWARHelper.GetIntrinsics(image.Width, image.Height);
j.width = image.Width;
j.height = image.Height;
HWARHelper.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();
}
}
public override IEnumerator Localize()
{
ARCameraImageBytes image = null;
if (m_Sdk.androidResolution == ImmersalSDK.CameraResolution.Max)
{
try
{
image = ARFrame.AcquirPreviewImageBytes();
}
catch (NullReferenceException e)
{
Debug.LogError("Cannot acquire FullHD image: " + e.Message);
image = ARFrame.AcquireCameraImageBytes();
}
}
else
{
image = ARFrame.AcquireCameraImageBytes();
}
if (image != null && image.IsAvailable)
{
stats.localizationAttemptCount++;
Vector3 camPos = m_Cam.transform.position;
Quaternion camRot = m_Cam.transform.rotation;
byte[] pixels;
Vector4 intrinsics = HWARHelper.GetIntrinsics();
HWARHelper.GetPlaneData(out pixels, image);
image.Dispose();
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, pixels));
});
while (!t.IsCompleted)
{
yield return(null);
}
int mapId = t.Result;
if (mapId >= 0 && ARSpace.mapIdToOffset.ContainsKey(mapId))
{
if (mapId != lastLocalizedMapId)
{
if (m_ResetOnMapChange)
{
foreach (KeyValuePair <Transform, SpaceContainer> item in ARSpace.transformToSpace)
{
item.Value.filter.ResetFiltering();
}
}
lastLocalizedMapId = mapId;
}
HWARHelper.GetRotation(ref rot);
MapOffset mo = ARSpace.mapIdToOffset[mapId];
float elapsedTime = Time.realtimeSinceStartup - startTime;
Debug.Log(string.Format("Relocalised in {0} seconds", elapsedTime));
stats.localizationSuccessCount++;
Matrix4x4 cloudSpace = mo.offset * Matrix4x4.TRS(pos, rot, Vector3.one);
Matrix4x4 trackerSpace = Matrix4x4.TRS(camPos, camRot, Vector3.one);
Matrix4x4 m = trackerSpace * (cloudSpace.inverse);
mo.space.filter.RefinePose(m);
}
}
yield return(StartCoroutine(base.Localize()));
}