Runtime/Scripts/WorldAnalysisARFoundation.cs

 using System.Collections.Generic;
+using System;
 using UnityEngine;
 using UnityEngine.XR.ARFoundation;
-using UnityEngine.XR.ARSubsystems;
 using ETSI.ARF.OpenAPI.WorldAnalysis;
 using static WorldAnalysisInterface;
+using System.Collections;
+using System.Linq;
+using ETSI.ARF.OpenAPI.WorldStorage;
+using ETSI.ARF.WorldStorage.REST;
 
 //Implementation of the WorldAnalysis interface
 public class WorldAnalysisARFoundation : MonoBehaviour, WorldAnalysisInterface
 {
+    /// <summary>
+    /// Dictionnary of susbscription informations for poses, for each item, stored using the UUID of the item (anchor/trackable)
+    /// </summary>
+    private Dictionary<Guid, SubscriptionInfo> m_subscriptionsPoses;
+    /// <summary>
+    /// Dictionnary of relocalization informations for each item, stored using their UUID
+    /// </summary>
+    private Dictionary<Guid, RelocalizationInformation> m_relocalizationInformations;
+    /// <summary>
+    /// Dictionnary of poses for each item, stored using their UUID
+    /// </summary>
+    private Dictionary<Guid, ETSI.ARF.OpenAPI.WorldAnalysis.Pose> m_computedPoses;
+
+    /// <summary>
+    /// Module Image
+    /// </summary>
+    private WorldAnalysisARFoundationModuleImage m_imageTrackableModule;
+
+    /// <summary>
+    /// World Storage Info object
+    /// </summary>
+    private WorldStorageInfo m_storageInfo;
+
+    /// <summary>
+    /// WorldStorageServer
+    /// </summary>
+    private ETSI.ARF.WorldStorage.WorldStorageServer m_worldStorageServer;
+
+    /// <summary>
+    /// Manages the AR Camera system
+    /// </summary>
+    private ARCameraManager m_arCameraManager;
+
+    /// <summary>
+    /// Is true if the ArSession.CheckAvailability method is running
+    /// </summary>
+    private bool isChekingAvailabilityRunning = false;
+
+    /// <summary>
+    /// Informations regarding a subscription to a pose
+    /// </summary>
+    public struct SubscriptionInfo
+    {
+        public Guid uuidTarget; // id trackable or anchor
+        public float timeValidity; //The duration of the validity of the subscription
+        public ETSI.ARF.OpenAPI.WorldAnalysis.Pose pose;
+        public PoseCallback callback;
+    }
 
     #region Unity_Methods
 
@@ -16,13 +68,42 @@ public class WorldAnalysisARFoundation : MonoBehaviour, WorldAnalysisInterface
     /// </summary>
     protected void Awake()
     {
+        Instance = this;
+        m_relocalizationInformations = new Dictionary<Guid, ETSI.ARF.OpenAPI.WorldStorage.RelocalizationInformation>();
+        m_computedPoses = new Dictionary<Guid, ETSI.ARF.OpenAPI.WorldAnalysis.Pose>();
+        m_subscriptionsPoses = new Dictionary<Guid, SubscriptionInfo>();
+        m_imageTrackableModule = new WorldAnalysisARFoundationModuleImage();
+        m_imageTrackableModule.Initialize();
+
+        m_storageInfo = FindObjectOfType<WorldStorageInfo>();
+        m_worldStorageServer = m_storageInfo.worldStorageServer;
     }
 
     /// <summary>
     /// Unity Start Method
     /// </summary>
-    protected void Start()
+    IEnumerator Start()
     {
+        isChekingAvailabilityRunning = true;
+        ///Wait until AR session is launched
+        yield return ARSession.CheckAvailability();
+
+        if (ARSession.state == ARSessionState.NeedsInstall)
+        {
+            Debug.Log("ETSI ARF : AR is supported on this device, but requires installation.");
+        }
+        else if (ARSession.state == ARSessionState.Unsupported)
+        {
+            Debug.Log("ETSI ARF : AR is not supported on this device.");
+        }
+        else
+        {
+            Debug.Log("ETSI ARF : AR is supported on this device.");
+        }
+
+        m_arCameraManager = FindObjectOfType<ARCameraManager>();
+
+        isChekingAvailabilityRunning = false;
     }
 
     /// <summary>
@@ -30,74 +111,499 @@ public class WorldAnalysisARFoundation : MonoBehaviour, WorldAnalysisInterface
     /// </summary>
     protected void Update()
     {
+        if (!isChekingAvailabilityRunning)
+        {
+            UnityEngine.Vector3 cameraTransformPos = m_arCameraManager.transform.position;
+
+            /// Then compute poses
+            foreach (KeyValuePair<Guid, SubscriptionInfo> subPose in m_subscriptionsPoses)
+            {
+                RelocalizationInformation information = m_relocalizationInformations[subPose.Value.uuidTarget];
+                RelocObjects firstRelocInfo = information.RelocObjects.First();
+                Dictionary<Guid, RelocObjects> temp = information.RelocObjects.ToDictionary(relocObject => relocObject.Trackable.UUID, relocObject => relocObject);
+
+                /// Trackable selection
+
+                Guid bestTrackableCandidateID = Guid.Empty;
+
+                List<RelocObjects> trackedCandidates = new List<RelocObjects>();
+                List<RelocObjects> limitedTrackingCandidates = new List<RelocObjects>();
+                List<RelocObjects> notTrackedCandidates = new List<RelocObjects>();
+
+                //Hierarchy of types
+                string[] types = { "MESH", "IMAGE_MARKER", "FIDUCIAL_MARKER", "MAP", "OTHER" }; //TODO : GEOPOSE
+
+                //We fill in the confidence level lists.
+                foreach (KeyValuePair<Guid, RelocObjects> relocObject in temp)
+                {
+                    WorldAnalysisARFoundationModule.TrackableInfo inf = m_imageTrackableModule.GetPoseTrackable(relocObject.Value.Trackable.Name); // for now only image module
+                    if (inf != null)
+                    {
+                        if (inf.confidence == 100)
+                        {
+                            trackedCandidates.Add(relocObject.Value);
+                        }
+                        else if (inf.confidence == 50)
+                        {
+                            limitedTrackingCandidates.Add(relocObject.Value);
+                        }
+                        else
+                        {
+                            notTrackedCandidates.Add(relocObject.Value);
+                        }
+                    }
+                }
+
+                //uses the types[] array indexes as key, and UUIDs as values
+                List<KeyValuePair<string, Guid>> typesSortedList = new List<KeyValuePair<string, Guid>>();
+                foreach (var relocObject in temp)
+                {
+                    typesSortedList.Add(new KeyValuePair<string, Guid>(relocObject.Value.Trackable.TrackableType.ToString(), relocObject.Value.Trackable.UUID));
+                }
+                var sortedItems = typesSortedList.OrderBy(item => Array.IndexOf(types, item.Key)).ToList();
+
+
+                if (trackedCandidates.Count > 0)
+                {
+                    bestTrackableCandidateID = SelectTrackableWithTypeAndDistance(trackedCandidates, typesSortedList, cameraTransformPos);
+                }
+                else if (limitedTrackingCandidates.Count > 0)
+                {
+                    bestTrackableCandidateID = SelectTrackableWithTypeAndDistance(limitedTrackingCandidates, typesSortedList, cameraTransformPos);
+                }
+                else if (notTrackedCandidates.Count > 0)
+                {
+                    bestTrackableCandidateID = SelectTrackableWithTypeAndDistance(notTrackedCandidates, typesSortedList, cameraTransformPos);
+                }
+
+                if (bestTrackableCandidateID != Guid.Empty)
+                {
+                    /// We have found the best trackable
+                    WorldAnalysisARFoundationModule.TrackableInfo info = m_imageTrackableModule.GetPoseTrackable(temp[bestTrackableCandidateID].Trackable.Name);
+                    if (info == null)
+                    {
+                        // For now we just ignore it : we could also send not tracked todo
+                        continue;
+                    }
+
+                    Matrix4x4 tr = WorldAnalysisUnityHelper.ConvertETSIARFTransform3DToUnity(firstRelocInfo.Transform3D);
+                    UnityEngine.Vector3 tr_trans = WorldAnalysisUnityHelper.ExtractTranslationFromMatrix(tr);
+                    UnityEngine.Quaternion tr_rot = WorldAnalysisUnityHelper.ExtractRotationFromMatrix(tr);
+
+                    UnityEngine.Vector3 newPos = info.position + info.rotation * tr_trans;
+                    UnityEngine.Quaternion newRot = (info.rotation * tr_rot);
+
+                    if (subPose.Value.pose.Mode == Mode_WorldAnalysis.TRACKABLES_TO_DEVICE)
+                    {
+                        // Inverse
+                        newPos = -newPos;
+                        newRot = UnityEngine.Quaternion.Inverse(newRot);
+                    }
+
+                    //Pose metadata
+                    ETSI.ARF.OpenAPI.WorldAnalysis.Pose poseToUpdate = new ETSI.ARF.OpenAPI.WorldAnalysis.Pose();
+                    poseToUpdate.Confidence = info.confidence;
+                    poseToUpdate.Uuid = subPose.Value.uuidTarget;
+                    poseToUpdate.Timestamp = info.timeStamp;
+                    poseToUpdate.EstimationState = info.state;
+                    poseToUpdate.InstructionInfo = ""; // not supported for now
+                    poseToUpdate.Mode = subPose.Value.pose.Mode;
+
+                    //Pose value
+                    VectorQuaternionPoseValue newPoseValue = new VectorQuaternionPoseValue();
+                    newPoseValue.Type = PoseValueType.VECTOR_QUATERNION;
+                    newPoseValue.Unit = ETSI.ARF.OpenAPI.WorldAnalysis.UnitSystem.M;
+
+                    newPoseValue.Position = new ETSI.ARF.OpenAPI.WorldAnalysis.Vector3(newPos.x, newPos.y, newPos.z);
+                    newPoseValue.Rotation = new ETSI.ARF.OpenAPI.WorldAnalysis.Quaternion(newRot.x, newRot.y, newRot.z, newRot.w);
+                    poseToUpdate.Value = newPoseValue;
+
+                    //Save pose
+                    m_computedPoses[subPose.Value.uuidTarget] = poseToUpdate; // Todo : in case of multiple subscription on the same pose, we compute it multiple times (can be needed in case of multiple mode, units etc.), in that case here the last one overrides
+
+                    if (subPose.Value.callback != null)
+                    {
+                        subPose.Value.callback(PoseEstimationResult.OK, poseToUpdate);
+                    }
+                }
+            }
+            ManageSubscriptionValidity();
+        }
+        else
+        {
+            Debug.Log("ETSI ARF : Starting...");
+        }
     }
 
     #endregion
 
+    #region Lifecycle
+
+    /// <summary>
+    /// Check the validity of all subscriptions and delete one if needed
+    /// </summary>
+    protected void ManageSubscriptionValidity()
+    {
+        List<Guid> subscriptionToDelete = new List<Guid>();
+        foreach (KeyValuePair<Guid, SubscriptionInfo> sub in m_subscriptionsPoses)
+        {
+            float validity = sub.Value.timeValidity;
+            if (Time.time > validity)
+            {
+                subscriptionToDelete.Add(sub.Key);
+            }
+        }
+        foreach (Guid s in subscriptionToDelete)
+        {
+            Debug.Log("ETSI ARF : Subscription deleted " + s);
+            m_subscriptionsPoses.Remove(s);
+        }
+    }
+
+    #endregion
 
     #region ARF_API
 
-    public  AskFrameRateResult SetPoseEstimationFramerate(string token, PoseConfigurationTrackableType type, EncodingInformationStructure encodingInformation, int minimumFramerate)
+    public AskFrameRateResult SetPoseEstimationFramerate(string token, PoseConfigurationTrackableType type, ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructure encodingInformation, int minimumFramerate)
     {
         return AskFrameRateResult.NOT_SUPPORTED; ///We cannot set any framerate for tracking on ARKit and ARCore
     }
 
-    public PoseEstimationResult GetLastPose(string token, string uuid, Mode_WorldAnalysis mode, out ETSI.ARF.OpenAPI.WorldAnalysis.Pose pose)
+    public PoseEstimationResult GetLastPose(string token, Guid uuid, Mode_WorldAnalysis mode, out ETSI.ARF.OpenAPI.WorldAnalysis.Pose pose)
     {
-        pose = null;
+        pose = new ETSI.ARF.OpenAPI.WorldAnalysis.Pose();
+        if (!m_computedPoses.ContainsKey(uuid))
+        {
+            Debug.LogWarning("ETSI ARF : Ask for the pose for an uuid without associated subscription");
+            return PoseEstimationResult.UNKNOWN_ID;
+        }
+        ETSI.ARF.OpenAPI.WorldAnalysis.Pose computedPose = m_computedPoses[uuid];
+        pose = computedPose;
+        if (mode != pose.Mode)
+        {
+            // Inverse pose
+            VectorQuaternionPoseValue poseValue = (VectorQuaternionPoseValue)pose.Value;
+            UnityEngine.Vector3 toUnityPos = WorldAnalysisUnityHelper.ConvertETSIVector3ToUnity(poseValue.Position);
+            toUnityPos = -toUnityPos;
+            UnityEngine.Quaternion toUnityRot = WorldAnalysisUnityHelper.ConvertETSIARFQuaternionToUnity(poseValue.Rotation);
+            toUnityRot = UnityEngine.Quaternion.Inverse(toUnityRot);
+
+            poseValue.Position = new ETSI.ARF.OpenAPI.WorldAnalysis.Vector3(toUnityPos.x, toUnityPos.y, toUnityPos.z);
+            poseValue.Rotation = new ETSI.ARF.OpenAPI.WorldAnalysis.Quaternion(toUnityRot.x, toUnityRot.y, toUnityRot.z, toUnityRot.w);
+        }
         return PoseEstimationResult.OK;
     }
 
 
-    public PoseEstimationResult[] GetLastPoses(string token, string[] uuids, Mode_WorldAnalysis [] modes, out ETSI.ARF.OpenAPI.WorldAnalysis.Pose[] poses)
+    public PoseEstimationResult[] GetLastPoses(string token, Guid[] uuids, Mode_WorldAnalysis[] modes, out ETSI.ARF.OpenAPI.WorldAnalysis.Pose[] poses)
     {
-        poses = null;
-        return null;
+        if (uuids.Length != modes.Length)
+        {
+            Debug.LogError("ETSI ARF : Get poses: uuids and modes array do no have the same length");
+            poses = null;
+            return null;
+        }
+        PoseEstimationResult[] resul = new PoseEstimationResult[uuids.Length];
+        poses = new ETSI.ARF.OpenAPI.WorldAnalysis.Pose[uuids.Length];
+        for (int i = 0; i < uuids.Length; i++)
+        {
+            ETSI.ARF.OpenAPI.WorldAnalysis.Pose pose;
+            PoseEstimationResult poseResul = GetLastPose(token, uuids[i], modes[i], out pose);
+            resul[i] = poseResul;
+            poses[i] = pose;
+        }
+        return resul;
     }
 
-    public InformationSubscriptionResult SubscribeToPose(string token, string uuid, Mode_WorldAnalysis mode, PoseCallback callback, ref int validity, out string subscriptionUUID)
+
+    public InformationSubscriptionResult SubscribeToPose(string token, Guid uuid, Mode_WorldAnalysis mode, PoseCallback callback, ref int validity, out Guid subscriptionUUID)
     {
-        subscriptionUUID = "";
-        return InformationSubscriptionResult.OK;
+        RelocalizationInformation relocInfo = null;
+        if (!m_relocalizationInformations.ContainsKey(uuid))
+        {
+            // In case of another subscription to the same uuid : we do not get the reloc information again
+            List<Guid> uuids = new List<Guid>
+            {
+               uuid
+            };
+
+            List<Mode_WorldStorage> modes = new List<Mode_WorldStorage>
+            {
+                Mode_WorldStorage.TRACKABLES_TO_REQUEST
+            };
+
+            ETSI.ARF.OpenAPI.WorldAnalysis.Capability[] currentCapabilities;
+            // Filter with capabilities of current world analysis
+            GetCapabilities(token, out currentCapabilities);
+
+            ///Convert into world storage capabilities
+            List<ETSI.ARF.OpenAPI.WorldStorage.Capability> capabilities = new List<ETSI.ARF.OpenAPI.WorldStorage.Capability>();
+            foreach (ETSI.ARF.OpenAPI.WorldAnalysis.Capability capability in currentCapabilities)
+            {
+                capabilities.Add(WorldAnalysisUnityHelper.ConvertWorldAnalysisCapability(capability));
+            }
+
+            /// Collect relocalization information
+            ETSI.ARF.OpenAPI.WorldStorage.Response response = RelocalizationInformationRequest.GetRelocalizationInformation(m_worldStorageServer, uuids, modes, capabilities);
+            relocInfo = response.RelocInfo.First(); //Only one uuid requested
+        }
+        else
+        {
+            relocInfo = m_relocalizationInformations[uuid];
+        }
+
+        if (relocInfo == null)
+        {
+            subscriptionUUID = Guid.Empty;
+            /// Subscription not possible
+            return InformationSubscriptionResult.NONE;
+        }
+
+        return CreateSubscriptionWithRelocalizationInformation(relocInfo, uuid, mode, callback, ref validity, out subscriptionUUID);
     }
 
-    public InformationSubscriptionResult[] SubscribeToPoses(string token, string[] uuids, Mode_WorldAnalysis [] modes, PoseCallback callback, ref int validity, out string[] subscriptionUUIDs)
+    public InformationSubscriptionResult[] SubscribeToPoses(string token, Guid[] uuids, Mode_WorldAnalysis[] modes, PoseCallback callback, ref int validity, out Guid[] subscriptionUUIDs)
     {
-        subscriptionUUIDs = null;
-        return null;
+        InformationSubscriptionResult[] resul = new InformationSubscriptionResult[uuids.Length];
+        for (int i = 0; i < uuids.Length; i++)
+        {
+            resul[i] = InformationSubscriptionResult.NONE; // Init with None in case we do not collect relocalization info for that id
+        }
+        subscriptionUUIDs = new Guid[uuids.Length];
+        if (uuids.Length != modes.Length)
+        {
+            Debug.LogError("ETSI ARF : You tried to subscribe to poses with two arrray of different length uuids and modes");
+            return resul;
+        }
+
+        ETSI.ARF.OpenAPI.WorldAnalysis.Capability[] currentCapabilities;
+        // Filter with capabilities of current world analysis
+        GetCapabilities(token, out currentCapabilities);
+
+        ///Convert into world storage capabilities
+        List<ETSI.ARF.OpenAPI.WorldStorage.Capability> capabilities = new List<ETSI.ARF.OpenAPI.WorldStorage.Capability>();
+        foreach (ETSI.ARF.OpenAPI.WorldAnalysis.Capability capability in currentCapabilities)
+        {
+            capabilities.Add(WorldAnalysisUnityHelper.ConvertWorldAnalysisCapability(capability));
+        }
+
+        List<Guid> l_uuids = new List<Guid>();
+        List<Mode_WorldStorage> l_modes = new List<Mode_WorldStorage>();
+        foreach (Guid uuid in uuids)
+        {
+            l_uuids.Add(uuid);
+            l_modes.Add(Mode_WorldStorage.TRACKABLES_TO_REQUEST);
+        }
+        /// Collect relocalization information
+        ETSI.ARF.OpenAPI.WorldStorage.Response response = RelocalizationInformationRequest.GetRelocalizationInformation(m_worldStorageServer, l_uuids, l_modes, capabilities);
+
+        /// Check every reloc information object
+        foreach (RelocalizationInformation relocInfo in response.RelocInfo)
+        {
+            int indexUUID = l_uuids.IndexOf(relocInfo.RequestUUID);
+            Guid subscriptionUUID;
+            /// Create a subscription for each found reloc information
+            resul[indexUUID] = CreateSubscriptionWithRelocalizationInformation(relocInfo, relocInfo.RequestUUID, modes[indexUUID], callback, ref validity, out subscriptionUUID);
+            subscriptionUUIDs[indexUUID] = subscriptionUUID;
+        }
+        return resul;
     }
 
-    public InformationSubscriptionResult GetSubsription(string token, string subscriptionUUID, out PoseCallback callback, out string target, out Mode_WorldAnalysis mode, out int validity)
+    public InformationSubscriptionResult GetSubsription(string token, Guid subscriptionUUID, out PoseCallback callback, out Guid target, out Mode_WorldAnalysis mode, out int validity)
     {
-        callback = null;
-        target = "";
-        mode = Mode_WorldAnalysis.TRACKABLES_TO_DEVICE;
-        validity = 0;
-        return InformationSubscriptionResult.OK;
+        if (!m_subscriptionsPoses.ContainsKey(subscriptionUUID))
+        {
+            subscriptionUUID = Guid.Empty;
+            callback = null;
+            target = Guid.Empty;
+            mode = Mode_WorldAnalysis.TRACKABLES_TO_DEVICE;
+            validity = 0;
+            return InformationSubscriptionResult.UNKNOWN_ID;
+        }
+        else
+        {
+            SubscriptionInfo subInfo = m_subscriptionsPoses[subscriptionUUID];
+            callback = subInfo.callback;
+            target = subInfo.uuidTarget;
+            mode = subInfo.pose.Mode;
+            float validitySeconds = subInfo.timeValidity - Time.time;
+            validity = (int)validitySeconds * 1000;// conversion in ms
+            return InformationSubscriptionResult.OK;
+        }
     }
 
-    public InformationSubscriptionResult UpdateSubscription(string token, string subscriptionUUID, Mode_WorldAnalysis mode, int validity, PoseCallback callback)
+    public InformationSubscriptionResult UpdateSubscription(string token, Guid subscriptionUUID, Mode_WorldAnalysis mode, int validity, PoseCallback callback)
     {
-        return InformationSubscriptionResult.OK;
+        if (m_subscriptionsPoses.ContainsKey(subscriptionUUID))
+        {
+            SubscriptionInfo currrent = m_subscriptionsPoses[subscriptionUUID];
+            SubscriptionInfo newSubInfo = new SubscriptionInfo();
+            newSubInfo.callback = callback;
+            newSubInfo.uuidTarget = currrent.uuidTarget;
+            newSubInfo.pose = new ETSI.ARF.OpenAPI.WorldAnalysis.Pose();
+            newSubInfo.pose.Mode = mode;
+            newSubInfo.timeValidity = Time.time + (validity / 1000.0f);
+            m_subscriptionsPoses[subscriptionUUID] = newSubInfo;
+            return InformationSubscriptionResult.OK;
+        }
+        else
+        {
+            return InformationSubscriptionResult.UNKNOWN_ID;
+        }
     }
 
-    public InformationSubscriptionResult UnSubscribeToPose(string subscriptionUUID)
+    public InformationSubscriptionResult UnSubscribeToPose(Guid subscriptionUUID)
     {
-        return InformationSubscriptionResult.OK;
+        if (m_subscriptionsPoses.ContainsKey(subscriptionUUID))
+        {
+            m_subscriptionsPoses.Remove(subscriptionUUID);
+            return InformationSubscriptionResult.OK;
+        }
+        else
+        {
+            return InformationSubscriptionResult.UNKNOWN_ID;
+        }
     }
 
-    public CapabilityResult GetCapabilities(string token, out Capability[] capabilities)
+    public CapabilityResult GetCapabilities(string token, out ETSI.ARF.OpenAPI.WorldAnalysis.Capability[] capabilities)
     {
-        capabilities = null;
+        List<ETSI.ARF.OpenAPI.WorldAnalysis.Capability> capabilitiesList = new List<ETSI.ARF.OpenAPI.WorldAnalysis.Capability>();
+
+        /// Image Tracking
+        if (m_imageTrackableModule != null)
+        {
+            ETSI.ARF.OpenAPI.WorldAnalysis.Capability capabilityImage = new ETSI.ARF.OpenAPI.WorldAnalysis.Capability();
+            capabilityImage.TrackableType = ETSI.ARF.OpenAPI.WorldAnalysis.TrackableType.IMAGE_MARKER;
+            ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructure encodingInformation = new ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructure();
+            encodingInformation.DataFormat = ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructureDataFormat.OTHER;
+            encodingInformation.Version = "1.01";
+            capabilityImage.EncodingInformation = encodingInformation;
+            capabilityImage.Framerate = 30; // Not particularly specified on ARKit and ARCore
+            capabilityImage.Latency = 0; // Not particularly specified on ARKit and ARCore
+            capabilityImage.Accuracy = 1; // Not particularly specified on ARKit and ARCore
+            capabilitiesList.Add(capabilityImage);
+        }
+
+        capabilities = capabilitiesList.ToArray();
         return CapabilityResult.OK;
     }
 
-    public CapabilityResult GetCapability(string token, string uuid, out bool isSupported, out TypeWorldStorage type, out Capability capability)
+    public CapabilityResult GetCapability(string token, Guid uuid, out bool isSupported, out ETSI.ARF.OpenAPI.WorldAnalysis.TypeWorldStorage type, out ETSI.ARF.OpenAPI.WorldAnalysis.Capability[] capability)
     {
-        isSupported = false;
-        type = TypeWorldStorage.UNKNOWN;
-        capability = null;
-        return CapabilityResult.OK;
+        throw new NotImplementedException();
+        /*   isSupported = false;
+           type = ETSI.ARF.OpenAPI.WorldAnalysis.TypeWorldStorage.UNKNOWN;
+           capability = null;
+           return CapabilityResult.OK;*/
+    }
+
+    #endregion
+
+    #region Helper 
+
+    /// <summary>
+    /// To select the best trackable
+    /// </summary>
+    /// <param name="candidates"></param>
+    /// <param name="typesSortedList"></param>
+    /// <param name="cameraTransformPos"></param>
+    /// <returns></returns>
+    private Guid SelectTrackableWithTypeAndDistance(List<RelocObjects> candidates, List<KeyValuePair<string, Guid>> typesSortedList, UnityEngine.Vector3 cameraTransformPos)
+    {
+        float bestDistance = 100.0f; //valeur par dfaut  = grande distance ( changer)
+        Guid selectedTrackableUUID = Guid.Empty;
+
+        //if only one trackable is candidate
+        if (candidates.Count > 0)
+        {
+            if (candidates.Count == 1)
+            {
+                selectedTrackableUUID = candidates[0].Trackable.UUID; //selected candidate
+            }
+            else if (candidates.Count >= 2)
+            {
+                //if there is more than one candidate, whe must chose using the hierarchy of the trackables
+                //The list is already sorted, so whe just keep those who have the higher rank
+                List<Guid> bestTypeCandidates = new List<Guid>();
+
+                foreach (KeyValuePair<string, Guid> kvp in typesSortedList)
+                {
+                    if (kvp.Key == typesSortedList.First().Key)
+                    {
+                        bestTypeCandidates.Add(kvp.Value);
+                    }
+                    else
+                    {
+                        break;
+                    }
+                }
+
+                //Then if there is only one candidate with the higher rank 
+                if (bestTypeCandidates.Count == 1)
+                {
+                    selectedTrackableUUID = bestTypeCandidates[0]; //selected candidate
+                }
+                else
+                {
+                    // Shortest distance among those in the confidence class with the same best trackable type rank
+                    foreach (Guid distanceCandidate in bestTypeCandidates)
+                    {
+                        foreach (RelocObjects tmp in candidates)
+                        {
+                            // select the corresponding candidate from the list of candidates
+                            if (tmp.Trackable.UUID == distanceCandidate)
+                            {
+                                WorldAnalysisARFoundationModule.TrackableInfo inf = m_imageTrackableModule.GetPoseTrackable(tmp.Trackable.Name);
+
+                                float distance = UnityEngine.Vector3.Distance(cameraTransformPos, inf.position);
+
+                                if (distance <= bestDistance)
+                                {
+                                    bestDistance = distance;
+                                    selectedTrackableUUID = tmp.Trackable.UUID;
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        return selectedTrackableUUID;
+    }
+
+    private InformationSubscriptionResult CreateSubscriptionWithRelocalizationInformation(RelocalizationInformation relocInfo, Guid uuid, Mode_WorldAnalysis mode, PoseCallback callback, ref int validity, out Guid subscriptionUUID)
+    {
+        subscriptionUUID = Guid.Empty;
+        bool hasSupport = false;
+        foreach (RelocObjects relocObj in relocInfo.RelocObjects)
+        {
+            /// We try to find a supported trackable
+            if (relocObj.Trackable.TrackableType == ETSI.ARF.OpenAPI.WorldStorage.TrackableType.IMAGE_MARKER)
+            {
+                hasSupport |= m_imageTrackableModule.AddTrackable(relocObj.Trackable);
+            }
+        }
+
+        if (!hasSupport)
+        {
+            /// We did not find any trackable to track this uuid
+            subscriptionUUID = Guid.Empty;
+            return InformationSubscriptionResult.NOT_SUPPORTED;
+        }
+        // We found at least one trackable
+        m_relocalizationInformations[uuid] = relocInfo;
+
+        /// We add the subscription
+        SubscriptionInfo sub = new SubscriptionInfo();
+        sub.timeValidity = Time.time + (validity / 1000.0f);
+        sub.pose = new ETSI.ARF.OpenAPI.WorldAnalysis.Pose();
+        sub.pose.Mode = mode;
+        sub.uuidTarget = uuid;
+        sub.callback = callback;
+        subscriptionUUID = Guid.NewGuid();
+        m_subscriptionsPoses.Add(subscriptionUUID, sub);
+        return InformationSubscriptionResult.OK;
     }
 
     #endregion

Runtime/Scripts/WorldAnalysisARFoundationModule.cs

+using UnityEngine;
+
+public interface WorldAnalysisARFoundationModule
+{
+    /// <summary>
+    /// Informations about a trackable
+    /// </summary>
+    public class TrackableInfo
+    {
+        public string                                               name;
+        public int                                                  timeStamp;
+        public ETSI.ARF.OpenAPI.WorldAnalysis.PoseEstimationState   state;
+        public double                                               confidence;
+        public Vector3                                              position;
+        public Quaternion                                           rotation;
+        public ETSI.ARF.OpenAPI.WorldAnalysis.TrackableType         trackableType;
+    }
+
+    /// <summary>
+    /// Initialize tracking module
+    /// </summary>
+    public void Initialize();
+
+    /// <summary>
+    /// Add to the list of trackable that the module need to tracked
+    /// </summary>
+    /// <param name="trackable">Trackable with its parameters</param>
+    /// <returns>supported or not</returns>
+    public bool AddTrackable(ETSI.ARF.OpenAPI.WorldStorage.Trackable trackable);
+
+    /// <summary>
+    ///  
+    /// </summary>
+    /// <param name="name"></param>
+    public TrackableInfo GetPoseTrackable(string name);
+
+}
\ No newline at end of file

Runtime/Scripts/WorldAnalysisARFoundationModule.cs.meta

+fileFormatVersion: 2
+guid: 93834ff9465ac13478c829115e75ac90
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Runtime/Scripts/WorldAnalysisARFoundationModuleImage.cs

+using System;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.XR.ARFoundation;
+using UnityEngine.XR.ARSubsystems;
+using System.IO;
+using Unity.XR.CoreUtils;
+using static WorldAnalysisARFoundationModule;
+using ETSI.ARF.OpenAPI.WorldStorage;
+
+public class WorldAnalysisARFoundationModuleImage : WorldAnalysisARFoundationModule
+{
+    /// <summary>
+    /// ARFoundation Image tracking management
+    /// </summary>
+    private ARTrackedImageManager m_trackedImageManager;
+    /// <summary>
+    /// Name of all images that have been added to the library
+    /// </summary>
+    private List<string> m_trackedImageInLibrary; 
+    /// <summary>
+    /// List of tracked images with tracking infos
+    /// </summary>
+    private Dictionary<string,TrackableInfo> m_trackedImages = new Dictionary<string,TrackableInfo>();
+    /// <summary>
+    /// Default Constructor
+    /// </summary>
+    public WorldAnalysisARFoundationModuleImage() { }
+
+    /// <summary>
+    /// Initialize image tracking module
+    /// </summary>
+    public void Initialize()
+    {
+        XROrigin origin = UnityEngine.Object.FindAnyObjectByType<XROrigin>();
+        m_trackedImageManager = origin.gameObject.AddComponent<ARTrackedImageManager>();
+        m_trackedImageInLibrary = new List<string>();
+        m_trackedImageManager.trackedImagePrefab = (GameObject)Resources.Load("ARFImageTrackingPrefab");
+        m_trackedImageManager.trackedImagesChanged += OnTrackedImagesChanged;
+    }
+
+    /// <summary>
+    ///  found  : pourrait aussi track pas track, on pourrait aussi ajouter la cofiance si dispo dans l'API
+    /// </summary>
+    /// <param name="name">name of the image trackable</param>
+
+    public TrackableInfo GetPoseTrackable(string name)
+    {
+        if (m_trackedImages.ContainsKey(name))
+        {
+            return m_trackedImages[name];
+        }
+        else
+        {
+            return null; 
+        }
+    }
+
+    /// <summary>
+    /// Add a trackable : need to be an image
+    /// </summary>
+    /// <param name="trackable">Image trackable</param>
+    /// <returns>Supported or not</returns>
+    public bool AddTrackable(Trackable trackable)
+    {
+        /// Here : we don't check if the trackable is allready added, AddImageToLibrary does it
+        if (trackable.TrackableType != ETSI.ARF.OpenAPI.WorldStorage.TrackableType.IMAGE_MARKER)
+        {
+            return false;
+        }
+        AddImageToLibrary(trackable.Name, (float)trackable.TrackableSize[0]);
+        return true;
+    }
+
+    /// <summary>
+    /// Event manager for when an image is tracked
+    /// </summary>
+    /// <param name="eventArgs">the event</param>
+    private void OnTrackedImagesChanged(ARTrackedImagesChangedEventArgs eventArgs)
+    {
+        foreach (var trackedImage in eventArgs.updated)
+        {
+            Vector3 position = trackedImage.transform.position;
+            Quaternion rotation = trackedImage.transform.rotation;
+
+            TrackableInfo info = new TrackableInfo();
+            info.name = trackedImage.referenceImage.name;
+            if (trackedImage.trackingState == TrackingState.Tracking)
+            {
+                info.state = ETSI.ARF.OpenAPI.WorldAnalysis.PoseEstimationState.OK;
+                info.confidence = 100;
+            }
+            else if (trackedImage.trackingState == TrackingState.Limited)
+            {
+                info.state = ETSI.ARF.OpenAPI.WorldAnalysis.PoseEstimationState.OK;
+                info.confidence = 50;
+            }
+            else
+            {
+                info.state = ETSI.ARF.OpenAPI.WorldAnalysis.PoseEstimationState.FAILURE; //ADD NOT_TRACKED ?
+                info.confidence = 0;
+            }
+            info.timeStamp = unchecked((int)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds());
+            info.position = position;
+            info.rotation = rotation;
+            info.trackableType = ETSI.ARF.OpenAPI.WorldAnalysis.TrackableType.IMAGE_MARKER;
+            m_trackedImages[info.name] = info;
+        }
+    }
+
+    /// <summary>
+    /// Add a new image to the arfoundation library
+    /// </summary>
+    /// <param name="fileName">file name</param>
+    /// <param name="imageWidthInMeters">image width in meters</param>
+    /// <returns></returns>
+    protected bool AddImageToLibrary(string fileName, float imageWidthInMeters)
+    {
+        // check if image is in the library
+        if (m_trackedImageInLibrary.Contains(fileName))
+        {
+            Debug.Log("Image allready added to library");
+            return true;
+        }
+#if UNITY_EDITOR
+        string imagePath = Application.streamingAssetsPath + "/" + fileName;
+#else
+        string imagePath = Application.persistentDataPath  + "/" + fileName ;
+#endif
+
+        bool found = CheckImageExist(ref imagePath); // try to find in jpg or png
+        if (!found)
+        {
+            Debug.LogWarning("Try to Track Image " + fileName + "  but file not found");
+            return false; 
+        }
+        
+        //Load texture and add it to arfoundation library
+        var rawData = File.ReadAllBytes(imagePath);
+        Texture2D tex = new Texture2D(2, 2);
+        tex.LoadImage(rawData);
+        AddTextureToLibrary(tex, fileName, imageWidthInMeters);
+        return true;
+    }
+
+    /// <summary>
+    /// Add a texture2D to the list of tracked image of the ARFoundation image library
+    /// </summary>
+    /// <param name="imageToAdd">texture to track</param>
+    /// <param name="imageName">name of the trackable</param>
+    /// <param name="imageWidthInMeters">with of the image in meters</param>
+    private async void AddTextureToLibrary(Texture2D imageToAdd, string imageName, float imageWidthInMeters)
+    {
+        while (ARSession.state == ARSessionState.CheckingAvailability  || ARSession.state == ARSessionState.None)
+        {
+            await System.Threading.Tasks.Task.Delay(100);
+        }
+        IReferenceImageLibrary library;
+        if (m_trackedImageManager.referenceLibrary == null)
+        {
+            library = m_trackedImageManager.CreateRuntimeLibrary(); // Create library if it does not exist
+        }
+        else
+        {
+            library = m_trackedImageManager.referenceLibrary;
+        }
+
+        if (library is MutableRuntimeReferenceImageLibrary mutableLibrary)
+        {
+            if (!m_trackedImageInLibrary.Contains(imageName))
+            {
+                mutableLibrary.ScheduleAddImageWithValidationJob(imageToAdd, imageName, imageWidthInMeters); // check if that does not fail?
+                m_trackedImageInLibrary.Add(imageName);
+            }
+        }
+        else
+        {
+            Debug.LogError("Error adding image: Library is not mutableLibrary");
+        }
+        m_trackedImageManager.referenceLibrary = library;
+        if (!m_trackedImageManager.enabled) m_trackedImageManager.enabled = true; // Necessary?
+    }
+
+    /// <summary>
+    /// Check if image exist in jpg or png format and modify string with path
+    /// </summary>
+    /// <param name="path">path to check without extension</param>
+    /// <returns>image exists or not</returns>
+    private bool CheckImageExist(ref string path)
+    {
+        string tempPath = path + ".png";
+        if (File.Exists(tempPath))
+        {
+            path = tempPath;
+            return true; 
+        }
+        else
+        {
+            tempPath = path + ".jpg";
+            if (File.Exists(tempPath))
+            {
+                path = tempPath;
+                return true; 
+            }
+        }
+        return false; 
+    }
+}
\ No newline at end of file

Runtime/Scripts/WorldAnalysisARFoundationModuleImage.cs.meta

+fileFormatVersion: 2
+guid: 51002affc1ae7c24abffa916faae5451
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Runtime/etsi.isg.arf.worldanalysisarfoundation.asmdef

@@ -5,7 +5,8 @@
         "GUID:99fdaa6f193b69346bfc8863615f98f0",
         "GUID:065e856e9f9becb49abdf0cb6855a039",
         "GUID:a9420e37d7990b54abdef6688edbe313",
-        "GUID:92703082f92b41ba80f0d6912de66115"
+        "GUID:92703082f92b41ba80f0d6912de66115",
+        "GUID:dc960734dc080426fa6612f1c5fe95f3"
     ],
     "includePlatforms": [],
     "excludePlatforms": [],