Skip to content

Instantly share code, notes, and snippets.

@fabio914
Created June 19, 2022 08:23
Show Gist options
  • Save fabio914/335d2aa2d2227a1518eee89f5b003c68 to your computer and use it in GitHub Desktop.
Save fabio914/335d2aa2d2227a1518eee89f5b003c68 to your computer and use it in GitHub Desktop.
ARKit (iPhone/iPad) to Unity (Quest 2) calibration

This is how I get the current ARKit camera pose and send it to the Unity app (via a socket):

func sendPoseUpdate(with frame: ARFrame) {
    guard case .normal = frame.camera.trackingState else { return }

    let position = frame.camera.transform.columns.3

    let positionVector = Vector3(
        x: .init(position.x),
        y: .init(position.y),
        z: .init(position.z)
    )

    let rotation = Quaternion(rotationMatrix: SCNMatrix4(frame.camera.transform))

    DispatchQueue.main.async { [weak sender] in
        sender?.send(devicePose: Pose(position: positionVector, rotation: rotation))
    }
}

where Pose is defined as:

typealias Vector3 = simd_double3

typealias Quaternion = simd_quatd

extension Quaternion {
    
    init(rotationMatrix m: SCNMatrix4) {
        self.init(simd_double4x4(m))
    }
}

struct Pose {
    let position: Vector3
    let rotation: Quaternion

    init(position: Vector3, rotation: Quaternion) {
        self.position = position
        self.rotation = rotation
    }
}

and this is what I have on the Quest side of things (Unity):

[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct PosePayload {
    public float px;
    public float py;
    public float pz;
    public float qx;
    public float qy;
    public float qz;
    public float qw;

    public const int StructSize = 7 * sizeof(float);

    public OVRPose ToOVRPose() {
        OVRPose ovrPose = new OVRPose();
        ovrPose.position = new Vector3(px, py, pz);
        ovrPose.orientation = new Quaternion(qx, qy, qz, qw);
        return ovrPose.flipZ();
    }

    public Matrix4x4 ToMatrix4x4() {
        OVRPose flippedOvrPose = ToOVRPose();

        return Matrix4x4.TRS(
            flippedOvrPose.position, 
            flippedOvrPose.orientation, 
            new Vector3(1, 1, 1)
        );
    }
}

[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct PointPayload {
    public float x;
    public float y;
    public float z;

    public const int StructSize = 3 * sizeof(float);

    public Vector3 ToVector3() {
        return new Vector3(x, y, -1.0f * z);
    }
}

public class Calibration {

    private Matrix4x4 calibrationMatrix;

    // Assuming that the ARKit Pose and the other Pose match in the real-world.
    public Calibration(PosePayload arkitPose, Transform pose) {
        Matrix4x4 arkitTransform = arkitPose.ToMatrix4x4();
        this.calibrationMatrix = pose.localToWorldMatrix * arkitTransform.inverse;
    }

    public Vector3 Convert(PointPayload arkitPoint) {
        return calibrationMatrix.MultiplyPoint(arkitPoint.ToVector3());
    }

    public Matrix4x4 Convert(PosePayload arkitPose) {
        return calibrationMatrix * arkitPose.ToMatrix4x4();
    }
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment