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));
}
}
}
}
}
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 ***************************");
}
}
}
}
