示例#1
0
        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();
            }
        }
示例#2
0
        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();
            }
        }
示例#3
0
        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();
            }
        }
示例#4
0
        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();
            }
        }
示例#5
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();
            }
        }
示例#6
0
        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();
            }
        }