Search code examples
iosswiftipadaugmented-realityarkit

Extract yaw/pitch/roll from faceAnchor in ARKit


Using ARKit for facetracking, I get faceAnchor (ARFaceAnchor) as soon as the face is detected, which provides a simd_float4x4 matrix. I know about transformation matrices, and am also aware that the topic has been partially addressed (here: How to get values from simd_float4 in objective-c , and here: simd_float4x4 Columns), but is there a straighforward way to get yaw/pitch/rool values from the face anchor? (in order to feed my y/p/r values in the code below).

    func renderer(_ renderer: SCNSceneRenderer, didUpdate node: SCNNode, for anchor: ARAnchor) {
        let faceAnchor = anchor as? ARFaceAnchor
        let data = faceAnchor?.transform
        print(data!)
        let message = OSCMessage(
            OSCAddressPattern("/orientation"),
            yawValue,
            pitchValue,
            rollValue
        )
        client.send(message)
        print(message)
        
    }

FYI, OSCMessage comes from the SwiftOSC framework which is embedded in my project.


Solution

  • As there is apparently not (yet) a function provided by Apple for that purpose, it is a priori required to implement quaternion to euler angles computation. With these mathematical resources, and a radian-to-degrees conversion function, this can be implemented as an extension, as follows:

    extension matrix_float4x4 {
        // Function to convert rad to deg
        func radiansToDegress(radians: Float32) -> Float32 {
            return radians * 180 / (Float32.pi)
        }
        var translation: SCNVector3 {
           get {
               return SCNVector3Make(columns.3.x, columns.3.y, columns.3.z)
           }
        }
        // Retrieve euler angles from a quaternion matrix
        var eulerAngles: SCNVector3 {
            get {
                // Get quaternions
                let qw = sqrt(1 + self.columns.0.x + self.columns.1.y + self.columns.2.z) / 2.0
                let qx = (self.columns.2.y - self.columns.1.z) / (qw * 4.0)
                let qy = (self.columns.0.z - self.columns.2.x) / (qw * 4.0)
                let qz = (self.columns.1.x - self.columns.0.y) / (qw * 4.0)
    
                // Deduce euler angles
                /// yaw (z-axis rotation)
                let siny = +2.0 * (qw * qz + qx * qy)
                let cosy = +1.0 - 2.0 * (qy * qy + qz * qz)
                let yaw = radiansToDegress(radians:atan2(siny, cosy))
                // pitch (y-axis rotation)
                let sinp = +2.0 * (qw * qy - qz * qx)
                var pitch: Float
                if abs(sinp) >= 1 {
                    pitch = radiansToDegress(radians:copysign(Float.pi / 2, sinp))
                } else {
                    pitch = radiansToDegress(radians:asin(sinp))
                }
                /// roll (x-axis rotation)
                let sinr = +2.0 * (qw * qx + qy * qz)
                let cosr = +1.0 - 2.0 * (qx * qx + qy * qy)
                let roll = radiansToDegress(radians:atan2(sinr, cosr))
                
                /// return array containing ypr values
                return SCNVector3(yaw, pitch, roll)
                }
        }
    }