Exemplo n.º 1
0
        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();
            }
        }
Exemplo n.º 2
0
        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);
        }