How to know user's position in Surrounding Space in visionOS

Question

Working on user position tracking in visionOS within Immersive Space. Any insights or tips to navigate this? Docs seem elusive at the moment. I searched and found queryPose but Xcode throws error.

struct ImmersiveView : View {
    private let attachmentID = "viewID"

    var body: some View {
        RealityView { content, attachments in
            if let fixedScene = try? await Entity(named: "ImmersiveScene",
                                                     in: realityKitContentBundle) {               
                let wtp = WorldTrackingProvider()
                let session = ARKitSession()

                let anchor = AnchorEntity(.head)
                anchor.anchoring.trackingMode = .continuous
                fixedScene.setParent(anchor)
                content.add(anchor)                
            
                if let sceneAttachment = attachments.entity(for: attachmentID) {
                    fixedScene.addChild(sceneAttachment)
                }
            
                guard let env = try? await EnvironmentResource(named: "Directional")
                else { return }
                let iblComponent = ImageBasedLightComponent(source: .single(env),
                                                 intensityExponent: 10)
                fixedScene.components[ImageBasedLightComponent.self] = iblComponent
                fixedScene.components.set(ImageBasedLightReceiverComponent(imageBasedLight: fixedScene))
            
                fixedScene.transform.translation.z = -1.0
                fixedScene.transform.translation.y = 0.35
                fixedScene.transform.translation.x = 0.25
                anchor.name = "Attachments"
            }
        }
    } attachments: {
        Attachment(id: attachmentID) { 
    }
}

Answer 1

We can subscribe to scene event updates using the Combine framework which should synchronize the frames with the refresh rate. You can then query the world tracking device anchor transform defining the current pose:

import Combine

let session = ARKitSession()
let worldInfo = WorldTrackingProvider()
@State var sceneUpdateSubscription : Cancellable? = nil

...

var body: some View {
    RealityView { content in 
        try? await session.run([worldInfo])
        ...
        sceneUpdateSubscription = 
            content.subscribe(to: SceneEvents.Update.self) {event in
                guard let pose = 
                   worldInfo.queryDeviceAnchor(atTimestamp: CACurrentMediaTime()) 
                else { return }
                let dt = event.deltaTime // elapsed time
                let toDeviceTransform = pose.originFromAnchorTransform
                let devicePosition = toDeviceTransform.translation
                let deviceRotation = toDeviceTransform.upper3x3
                ...
            } as? any Cancellable
    }
    ...
}

The position of the camera is in the 4th column of the 4x4. You can fetch the rotation matrix in the upper 3x3 portion of the matrix:

extension simd_float4x4 {
    var translation : simd_float3 {
        return simd_float3(columns.3.x, columns.3.y, columns.3.z)
    }
    var upper3x3 : simd_float3x3 {
       return simd_float3x3(columns.0.float3, columns.1.float3, columns.2.float3)
    }
}

extension simd_float4 {
    var float3 : simd_float3 {
        return simd_float3(x,y,z)
    }
}

Answer 2

In Xcode 15.2 upper3x3 needs to be something like this to avoid "Value of type 'simd_float4' (aka 'SIMD4') has no member ‘float3'" errors.

extension simd_float4x4 {
var translation : simd_float3 {
    return simd_float3(columns.3.x, columns.3.y, columns.3.z)
}
var upper3x3 : simd_float3x3 {
    return simd_float3x3(simd_float3(columns.0.w, columns.0.y, columns.0.z), simd_float3(columns.1.x, columns.1.w, columns.1.z), simd_float3(columns.2.x, columns.2.y, columns.2.w))
}}

Answer 3

visionOS Camera Transform

Since the transform matrix of AnchorEntity(.head) is currently hidden in visionOS, use the DeviceAnchor object from ARKit framework. For that, run ARKitSession object, create DeviceAnchor, then call the originFromAnchorTransform instance property to get the 4x4 transform matrix from the device to the origin coordinate system.

import SwiftUI
import RealityKit
import ARKit

@main struct PoseXApp : App {
    var body: some Scene {
        ImmersiveSpace(id: "ImmersiveSpace") {
            ContentView()
        }
        .immersionStyle(selection: .constant(.mixed), in: .mixed)
    }
}

---

@Observable class VisionProPose {
    let session = ARKitSession()
    let worldTracking = WorldTrackingProvider()
    
    func runArSession() async {
        Task {
            try? await session.run([worldTracking])
        }
    }

    func getTransform() async -> simd_float4x4? {
        guard let deviceAnchor = worldTracking.queryDeviceAnchor(atTimestamp: 1)
        else { return nil }
    
        let transform = deviceAnchor.originFromAnchorTransform
        return transform
    }
}

Now you are able to register the values ​​of the transform matrix of the camera's anchor and transfer them to any entity in your scene using Timer updates (here 10 times per second).

struct ContentView : View {
    let visionProPose = VisionProPose()
    let box = ModelEntity(mesh: .generateBox(size: 0.2))
    
    var body: some View {
        RealityView { content in
            Task {
                await visionProPose.runArSession()
            }
            for i in 1...5 {
                let sphere = ModelEntity(mesh: .generateSphere(radius: 0.15))
                sphere.position.z -= Float(i)
                content.add(sphere)
            }
            content.add(box)
        }
        .onAppear {
            Timer.scheduledTimer(withTimeInterval: 0.1, repeats: true) { _ in
                Task {
                    let mtx = await visionProPose.getTransform()
                    print(mtx?.columns.3.z ?? 0.0)
                    
                    box.position = [Float((mtx?.columns.3.x)!),
                                    Float((mtx?.columns.3.y)!),
                                    Float((mtx?.columns.3.z)!) - 1.0 ]
                }
            }
        }
    }
}