Problem

Apple has an object for tracking the wrist such as anchor.originFromAnchorTransform and on this object, there is a rotation field that is protected so I can’t access it, but it gets printed if you print anchor like this. I’m trying to reproduce the values that HandAnchor has calculated from the transformation matrix but I can never get what the HandAnchor has calculated

HandAnchor(
 chirality: left,
 id: F37E231E-ED08-4E3A-8888-AD99CA814F70,
 isTracked: true, 
 originFromAnchorTransform:
   <translation=(-0.124833 0.765254 -0.135481) 
   rotation=(76.86° -119.53° 144.82°)>,
 handSkeleton: HandSkeleton(jointCount: 27))

Data

Here is the original 4×4 matrix. Note: Apple uses column major, so each array is a column.

simd_float4x4([
[-0.08531584, 0.13101268, -0.987703, 0.0],
[0.9765112, -0.18586183, -0.10900249, 0.0],
[-0.19785702, -0.97380245, -0.1120784, 0.0],
[-0.12483275, 0.7652545, -0.13548124, 1.0]])

These are the numbers I keep calculating based on the code below

(-96.56549, 11.411671, 94.99314)
(94.99314, 11.411671, -96.56549)
(-173.70236, 6.522802, -101.485016)

but the expected is

(76.86° -119.53° 144.82°)

Code

I’ve tried a bunch of different formulas, but no matter what, I can’t get the same calculations that it’s getting.

  func getIntrinsicRotation(_ anchor: HandAnchor) -> (Float, Float, Float) {
      let rotationMatrix = extractUpperLeft3x3(from: anchor.originFromAnchorTransform)
      
      // Compute yaw, pitch, roll from rotationMatrix in intrinsic order
      let sy = sqrt(rotationMatrix[0, 0] * rotationMatrix[0, 0] + rotationMatrix[1, 0] * rotationMatrix[1, 0])
      let singular = sy < 1e-6 // If singularity is detected
      
      let yaw: Float
      let pitch: Float
      let roll: Float

      if !singular {
          yaw = atan2(rotationMatrix[1, 0], rotationMatrix[0, 0])
          pitch = atan2(-rotationMatrix[2, 0], sy)
          roll = atan2(rotationMatrix[2, 1], rotationMatrix[2, 2])
      } else {
          yaw = atan2(-rotationMatrix[1, 2], rotationMatrix[1, 1])
          pitch = atan2(-rotationMatrix[2, 0], sy)
          roll = 0
      }

      // Convert radians to degrees
      let yawDegrees = yaw * 180 / .pi
      let pitchDegrees = pitch * 180 / .pi
      let rollDegrees = roll * 180 / .pi
      
      return (yawDegrees, pitchDegrees, rollDegrees)
  }
  
  func getExtrinsicRotation(_ anchor: HandAnchor) -> (Float, Float, Float) {
      let rotationMatrix = extractUpperLeft3x3(from: anchor.originFromAnchorTransform)
      
      // Compute roll, pitch, yaw from rotationMatrix in extrinsic order
      let sy = sqrt(rotationMatrix[0, 0] * rotationMatrix[0, 0] + rotationMatrix[1, 0] * rotationMatrix[1, 0])
      let singular = sy < 1e-6 // If singularity is detected
      
      let roll: Float
      let pitch: Float
      let yaw: Float

      if !singular {
          roll = atan2(rotationMatrix[1, 2], rotationMatrix[0, 2])
          pitch = atan2(-rotationMatrix[2, 2], sy)
          yaw = atan2(rotationMatrix[2, 1], rotationMatrix[2, 0])
      } else {
          roll = atan2(-rotationMatrix[1, 0], rotationMatrix[0, 0])
          pitch = atan2(-rotationMatrix[2, 0], sy)
          yaw = 0
      }

      // Convert radians to degrees
      let rollDegrees = roll * 180 / .pi
      let pitchDegrees = pitch * 180 / .pi
      let yawDegrees = yaw * 180 / .pi
      
      return (rollDegrees, pitchDegrees, yawDegrees)
  }
  
  func getRotation(_ anchor: HandAnchor) -> (Float, Float, Float) {
    let rotationMatrix = extractUpperLeft3x3(from: anchor.originFromAnchorTransform)
    
    // Compute roll, pitch, yaw from rotationMatrix
    let sy = sqrt(rotationMatrix[0, 0] * rotationMatrix[0, 0] + rotationMatrix[1, 0] * rotationMatrix[1, 0])
    let singular = sy < 1e-6 // If singularity is detected

    let x: Float
    let y: Float
    let z: Float

    if !singular {
        x = atan2(rotationMatrix[2, 1], rotationMatrix[2, 2])
        y = atan2(-rotationMatrix[2, 0], sy)
        z = atan2(rotationMatrix[1, 0], rotationMatrix[0, 0])
    } else {
        x = atan2(-rotationMatrix[1, 2], rotationMatrix[1, 1])
        y = atan2(-rotationMatrix[2, 0], sy)
        z = 0
    }

    // Convert radians to degrees
    let rollDegrees = x * 180 / .pi
    let pitchDegrees = y * 180 / .pi
    let yawDegrees = z * 180 / .pi
    
    return (rollDegrees, pitchDegrees, yawDegrees)
  }