Retrieving bone rotations from 3D Skeleton in ARKit 3 - swift

Question

I'm trying to get the bone rotations related to their parents, but I end up getting pretty weird angles.
I've tried everything, matrix multiplications, offsets, axis swapping, and no luck.
guard let bodyAnchor = anchor as? ARBodyAnchor else { continue }
let skeleton = bodyAnchor.skeleton
let jointTransforms = skeleton.jointLocalTransforms
for (i, jointTransform) in jointTransforms.enumerated() {
//RETRIEVE ANGLES HERE
}
In //RETRIEVE ANGLES HERE I've tried different approaches:
let n = SCNNode()
n.transform = SCNMatrix4(jointTransform)
print(n.eulerAngles)
In this try, I set the jointTransformation to a SCNNode.transform so I can retrieve the eulerAngles to make them human readable and try to understand what's happening.
I get to work some joints, but I think it's pure coincidence or luck, because the rest of the bones rotate very weird.
In other try I get them using jointModelTransforms (Model, instead of Local) so all transforms are relative to the Root bone of the Skeleton.
With this approach I do matrix multiplications like this:
LocalMatrix = Inverse(JointModelMatrix) * (ParentJointModelMatrix)
To get the rotations relative to its parent, but same situation, some bones rotate okay other rotate weird. Pure coincidence I bet.
Why do I want to get the bone rotations?
I'm trying build a MoCap app with my phone that passes to Blender the rotations, trying to build .BVH files from this, so I can use them on Blender.
This is my own rig:
I've done this before with Kinect, but I've been trying for days to do it on ARKit 3 with no luck :(

Answer 1

Using simd_quatf(from:to:) with the right input should do it. I had trouble with weird angles until i started normalising the vectors:
guard let bodyAnchor = anchor as? ARBodyAnchor else { continue }
let skeleton = bodyAnchor.skeleton
let jointTransforms = skeleton.jointLocalTransforms
for (i, jointTransform) in jointTransforms.enumerated() {
// First i filter out the root (Hip) joint because it doesn't have a parent
let parentIndex = skeleton.definition.parentIndices[i]
guard parentIndex >= 0 else { continue } // root joint has parent index of -1
//RETRIEVE ANGLES HERE
let jointVectorFromParent = simd_make_float3(jointTransform.columns.3)
let referenceVector: SIMD3<Float>
if skeleton.definition.parentIndices[parentIndex] >= 0 {
referenceVector = simd_make_float3(jointTransforms[parentIndex].columns.3)
} else {
// The parent joint is the Hip joint which should have
// a vector of 0 going to itself
// It's impossible to calculate an angle from a vector of length 0,
// So we're using a vector that's just pointing up
referenceVector = SIMD3<Float>(x: 0, y: 1, z: 0)
}
// Normalizing is important because simd_quatf gives weird results otherwise
let jointNormalized = normalize(jointVectorFromParent)
let referenceNormalized = normalize(referenceVector)
let orientation = simd_quatf(from: referenceNormalized, to: jointNormalized)
print("angle of joint \(i) = \(orientation.angle)")
}
One important thing to keep in mind though:
ARKit3 tracks only some joints (AFAIK the named joints in ARSkeleton.JointName). The other joints are extrapolated from that using a standardized skeleton. Which means, that the angle you get for the elbow for example won't be the exact angle the tracked persons elbow has there.

Answer 2

Just a guess… does this do the job?
let skeleton = bodyAnchor.skeleton
let jointTransforms = skeleton.jointLocalTransforms
for (i, jointTransform) in jointTransforms.enumerated() {
print(Transform(matrix: jointTransform).rotation)
}