Mapping Kinect data to armature

Question

I was attempting to improve a Blender addon for MakeHuman by directly interfacing with a xBox Kinect2 sensor. I also made an operator in the addon to convert their default skeleton to something that matches. It is manually pose-able, so hopefully not the main issue.

The capture part using a DLL I made went well. It sends back JSON formatted info. I am just not good enough to figure out how assign it to an armature, after trying many different things.

I made a .blend with the converted armature, and added a script (in a text editor window) with a single frame of data and one of the ways I tried to assign it. The script "works" in that it creates a single frame action with the data. What it does to the mesh is terrible, but I wanted to do as much leg work as possible.

The pose is just standing with arms crossed.

I should mention that Kinect has Y and Z flipped from Blender. The values are in meters. I already converted it to the decimeters of the mesh. Here is a doc (with picture) of the Kinect's Camera Space.

If someone could look at this, it would be great. Just to have a self contained question, here is the code of the script in the .blend.

import bpy
from json import loads
from mathutils import Matrix, Quaternion, Vector

json  = '{'
json += '"SpineBase": {"location": {"x":0.71,"y":8.82,"z":19.70},"rotation": {"x":-0.012,"y":0.999,"z":0.000,"w":0.043} },'
json += '"SpineMid": {"location": {"x":0.64,"y":11.77,"z":19.69},"rotation": {"x":-0.012,"y":1.000,"z":-0.000,"w":0.001} },'
json += '"Neck": {"location": {"x":0.69,"y":14.45,"z":19.52},"rotation": {"x":0.024,"y":0.997,"z":-0.052,"w":-0.041} },'
json += '"Head": {"location": {"x":0.37,"y":16.28,"z":19.80},"rotation": {"x":0.000,"y":0.000,"z":0.000,"w":0.000} },'
json += '"ShoulderLeft": {"location": {"x":-1.10,"y":13.00,"z":19.16},"rotation": {"x":0.836,"y":-0.538,"z":0.057,"w":-0.094} },'
json += '"ElbowLeft": {"location": {"x":-0.96,"y":13.56,"z":16.15},"rotation": {"x":-0.636,"y":-0.003,"z":-0.027,"w":0.771} },'
json += '"WristLeft": {"location": {"x":0.92,"y":13.73,"z":14.89},"rotation": {"x":-0.520,"y":-0.207,"z":-0.434,"w":0.706} },'
json += '"HandLeft": {"location": {"x":1.44,"y":13.95,"z":14.75},"rotation": {"x":-0.345,"y":-0.329,"z":-0.438,"w":0.762} },'
json += '"ShoulderRight": {"location": {"x":1.76,"y":14.05,"z":19.09},"rotation": {"x":0.607,"y":0.767,"z":-0.179,"w":-0.109} },'
json += '"ElbowRight": {"location": {"x":1.42,"y":11.88,"z":16.84},"rotation": {"x":0.900,"y":-0.135,"z":0.183,"w":-0.372} },'
json += '"WristRight": {"location": {"x":-0.18,"y":11.33,"z":15.28},"rotation": {"x":-0.518,"y":-0.029,"z":0.589,"w":0.619} },'
json += '"HandRight": {"location": {"x":-0.69,"y":11.26,"z":15.05},"rotation": {"x":-0.405,"y":0.093,"z":0.633,"w":0.654} },'
json += '"HipLeft": {"location": {"x":-0.05,"y":8.76,"z":19.25},"rotation": {"x":0.705,"y":-0.658,"z":0.188,"w":-0.184} },'
json += '"KneeLeft": {"location": {"x":-0.32,"y":4.51,"z":19.35},"rotation": {"x":0.737,"y":-0.032,"z":-0.675,"w":-0.013} },'
json += '"AnkleLeft": {"location": {"x":-0.38,"y":1.02,"z":19.61},"rotation": {"x":-0.635,"y":0.034,"z":0.771,"w":-0.018} },'
json += '"FootLeft": {"location": {"x":-0.56,"y":0.25,"z":18.76},"rotation": {"x":0.000,"y":0.000,"z":0.000,"w":0.000} },'
json += '"HipRight": {"location": {"x":1.46,"y":8.84,"z":19.37},"rotation": {"x":0.684,"y":0.701,"z":-0.144,"w":-0.140} },'
json += '"KneeRight": {"location": {"x":1.64,"y":4.60,"z":19.19},"rotation": {"x":0.676,"y":-0.002,"z":0.736,"w":-0.030} },'
json += '"AnkleRight": {"location": {"x":1.72,"y":0.97,"z":19.63},"rotation": {"x":0.676,"y":0.051,"z":0.735,"w":0.032} },'
json += '"FootRight": {"location": {"x":1.83,"y":0.28,"z":18.63},"rotation": {"x":0.000,"y":0.000,"z":0.000,"w":0.000} },'
json += '"SpineShoulder": {"location": {"x":0.66,"y":13.81,"z":19.59},"rotation": {"x":0.006,"y":0.999,"z":-0.024,"w":-0.042} },'
json += '"HandTipLeft": {"location": {"x":2.18,"y":14.01,"z":14.50},"rotation": {"x":0.000,"y":0.000,"z":0.000,"w":0.000} },'
json += '"ThumbLeft": {"location": {"x":1.39,"y":14.46,"z":14.98},"rotation": {"x":0.000,"y":0.000,"z":0.000,"w":0.000} },'
json += '"HandTipRight": {"location": {"x":-1.17,"y":11.07,"z":14.77},"rotation": {"x":0.000,"y":0.000,"z":0.000,"w":0.000} },'
json += '"ThumbRight": {"location": {"x":-1.33,"y":11.40,"z":15.07},"rotation": {"x":0.000,"y":0.000,"z":0.000,"w":0.000} }'
json += '}'
frame_data = loads(json)
armature = bpy.data.objects['Default_no_toes']

def assignBone(boneName, parentPoseMat):
    poseBone  = armature.pose.bones[boneName]
    restLocal = armature.data.bones[boneName].matrix_local.copy()
    isRootBone = parentPoseMat is None

    # construct a matrix from kinect data
    rot = frame_data[boneName]['rotation']
    kMat = Quaternion((rot['w'], rot['x'], rot['z'], rot['y'])).to_matrix().to_4x4()
    if isRootBone:                
        loc = frame_data[boneName]['location']
        kMat = kMat * Matrix.Translation((loc['x'], loc['z'], loc['y']))

    # convert to local coordinates
    kMatLocal = kMat * restLocal.inverted()

    # back out parent bone's pose, in parent's local terms
    kMatFinal = kMatLocal * parentPoseMat.inverted() if not isRootBone else kMatLocal

    # assign animation values 
    if isRootBone:
        poseBone.location = kMatFinal.to_translation()
        poseBone.keyframe_insert('location', frame = 1, group=boneName)

    poseBone.rotation_quaternion = kMatFinal.to_quaternion()
    poseBone.keyframe_insert('rotation_quaternion', frame = 1, group=boneName)

    for potentialKid in armature.pose.bones:
        if potentialKid.parent == poseBone:
            assignBone(potentialKid.name, kMatLocal)

assignBone("SpineBase", None)

Answer 1

I think I have solved this problem by solely using the captured location data, ignoring rotation, and saved my sanity at the same!

First, when a buffer of frames of one of the captured bodies is assigned, add an empty with the same name as the bone to the scene:

def makeEmpties(self):
    # Add all the empties in OBJECT Mode, one for each bone
    # use first frame of animation, not bones though, since user could have removed fingers
    bpy.ops.object.mode_set(mode='OBJECT')
    self.empties = {}
    for boneName in self.dataSets[0]:
         o = bpy.data.objects.new( boneName, None )
         o.empty_draw_size = 0.5
         o.empty_draw_type = 'ARROWS'
         o.show_name = True
         o.name = boneName

         bpy.context.scene.objects.link(o)
         self.empties[boneName] = o

Then for each frame, assign the location of the corresponding empty with the Y-Z switched data. For the root bone, 'SpinBase', I need to assign the tail not the head, so an offset of the length is also added (It is a horizontal bone in this rig):

def assignEmpties(self, bonesAnim):
    for boneName in bonesAnim:
        loc = bonesAnim[boneName]['location']

        empty = bpy.data.objects[boneName]

        empty.location = (loc['x'], loc['z'], loc['y'])              
        if boneName == 'SpineBase':
            empty.location.y += self.rootOffset

Then either a copy rotation or stretch constraint is added to each bone, and I got my first recorded pose. Should the constraints be added before the empties are positioned, a TOTAL mess is the result. Do not know yet if the constraints need to be deleted after each frame & re-added after repositioning, but I can live with that. There has to be a function to delete the constraints (as well as empties) at the end of the transfer anyway. Code:

def assignConstraints(self, armature):
    # assign a stretch to constraint to each bone of the empty of the same name
    # unless it is root, where a copy location is assigned
    bpy.ops.object.mode_set(mode='POSE')
    for bone in armature.pose.bones:

        if bone.parent is None or not armature.data.bones[bone.name].use_connect:
            constraint = bone.constraints.new('COPY_LOCATION')
            constraint.target = self.empties['SpineBase']
        else:
            constraint = bone.constraints.new('STRETCH_TO')
            constraint.target = self.empties[bone.name]

Here is the first frame of a capture. The fingers look bad, but with only 3 hand bones, that is going to be tough. I plan on deleting those bones: