version 1.3 : added position tracking

CHANGELOG.md

@@ -1,3 +1,6 @@
+Version 1.3:
+- added position tracking, and ability to lock position an each axis
+
 Version 1.2:
 - added smoothing with Kalman filter
 - added sensor tilt angle compensation

README.md

@@ -31,4 +31,4 @@ There are two parts in the project :
 The library was designed to be built using Visual Studio 2017 (only the Release configuration has been properly configured at the moment).
 
 ## Current progress
-The project is currently in version 1.1. 
+The project is currently in version 1.3. 

kinectMocap.cpp

@@ -37,7 +37,6 @@ int initSensor(double inDt) {
 	HRESULT hr;
 	int res = 0;
 	tilt = -100;
-	sensorH = -1;
 	dt = inDt;
 
 	hr = GetDefaultKinectSensor(&m_pKinectSensor);
@@ -133,13 +132,9 @@ int updateFrame() {
 		if (tilt == -100) {
 			// initialize tilt angle
 			Vector4 floorPlane;
-
+			tilt = 0;
 			if (SUCCEEDED(pBodyFrame->get_FloorClipPlane(&floorPlane))) {
 				tilt = atan2(floorPlane.z, floorPlane.y);
-				sensorH = floorPlane.w;
-				planeX = floorPlane.x;
-				planeY = floorPlane.y;
-				planeZ = floorPlane.z;
 			}
 		}
 
@@ -165,12 +160,10 @@ int updateFrame() {
 						if (SUCCEEDED(hr)) {
 							for (int j = 0; j < 25; j++) {
 								// compensate tilt
-								if (tilt != -100) {
-									double height = joints[j].Position.Y * cos(tilt) + joints[j].Position.Z * sin(tilt);
-									double depth = joints[j].Position.Z * cos(tilt) - joints[j].Position.Y * sin(tilt);
-									joints[j].Position.Y = height;
-									joints[j].Position.Z = depth;
-								}
+								double height = joints[j].Position.Y * cos(tilt) + joints[j].Position.Z * sin(tilt);
+								double depth = joints[j].Position.Z * cos(tilt) - joints[j].Position.Y * sin(tilt);
+								joints[j].Position.Y = height;
+								joints[j].Position.Z = depth;
 
 								// apply kalman filter to each joint
 								applyKalman(j);
@@ -189,16 +182,11 @@ int updateFrame() {
 	return res;
 }
 
-double getTilt() {
-	return tilt*180/3.141592653;
-}
-
 struct Sensor {
 	tuple getJoint(int jointNumber) { return make_tuple(joints[jointNumber].Position.X, joints[jointNumber].Position.Y, joints[jointNumber].Position.Z, static_cast<int>(joints[jointNumber].TrackingState)); }
 	int init(double dt) { return initSensor(dt); }
 	int close() { return closeSensor(); }
 	int update() { return updateFrame(); }
-	double tilt() { return getTilt(); }
 };
 
 BOOST_PYTHON_MODULE(kinectMocap4Blender) {
@@ -207,6 +195,5 @@ BOOST_PYTHON_MODULE(kinectMocap4Blender) {
 		.def("close", &Sensor::close)
 		.def("update", &Sensor::update)
 		.def("getJoint", &Sensor::getJoint)
-		.def("tilt", &Sensor::tilt)
 	;
 }

kinectMocap.h

@@ -31,7 +31,7 @@ using namespace boost::python;
 
 // Current Kinect
 IKinectSensor*          m_pKinectSensor;
-double					tilt, sensorH, planeX, planeY, planeZ;
+double					tilt;
 
 Joint					joints[JointType_Count];
 

scripts/2.79/kinect_mocap.py

@@ -23,7 +23,7 @@
     "name": "Kinect Motion Capture plugin",
     "description": "Motion capture using MS Kinect v2",
     "author": "Morgane Dufresne",
-    "version": (1, 2),
+    "version": (1, 3),
     "blender": (2, 79, 0),
     "warning": "You need a MS Kinect v2 sensor (XBox One)",
     "support": "COMMUNITY",
@@ -67,6 +67,12 @@ class KmcProperties(bpy.types.PropertyGroup):
     targetBones = bpy.props.CollectionProperty(type = KmcTarget)
     currentFrame = bpy.props.IntProperty(name="currentFrame", description="current recording frame", default=0)
     record = bpy.props.BoolProperty(name="Record captured motion", description="activate recording while tracking")
+    firstFramePosition = bpy.props.FloatVectorProperty(name="firstFramePosition", description="position of root bone in first frame", size=3)
+    initialOffset = bpy.props.FloatVectorProperty(name="initialOffset", description="position of root bone in rest pose", size=3)
+    lockHeight = bpy.props.BoolProperty(name="height", description="ignore vertical movement", default=True)
+    lockwidth = bpy.props.BoolProperty(name="width", description="ignore lateral movement", default=True)
+    lockDepth = bpy.props.BoolProperty(name="depth", description="ignore depth movement", default=True)
+
 
 jointType = {
     "SpineBase":0,
@@ -167,12 +173,19 @@ def initialize(context):
     bpy.ops.pose.transforms_clear()
     bpy.ops.pose.select_all(action=('DESELECT'))
     context.scene.kmc_props.currentFrame = 0
+    context.scene.kmc_props.firstFramePosition = (-1,-1,-1)
+    context.scene.kmc_props.initialOffset = (0,0,0)
+
 
     for target in context.scene.kmc_props.targetBones:
         if target.value is not None and target.value != "" :
             bone = bpy.data.objects[context.scene.kmc_props.arma_list].pose.bones[target.value]
             bone.rotation_mode = 'QUATERNION'
 
+            # Store initial position of spine0 bone
+            if target.name == "Spine0":
+                context.scene.kmc_props.initialOffset = bpy.data.objects[context.scene.kmc_props.arma_list].pose.bones[target.value].matrix.translation
+
             # Store rest pose angles for column, head and feet bones
             if bonesDefinition[target.name][2] is not None :
                 baseDir =  bonesDefinition[target.name][2] * bone.matrix
@@ -197,6 +210,26 @@ def updatePose(context, bone):
             if(head[3] == 2) and (tail[3] == 2) :
                 boneV = Vector((head[X] - tail[X], tail[Y] - head[Y], tail[Z] - head[Z]))
 
+                # if first bone, update position (only for configured axes)
+                if target.name == "Spine0":
+                    # initialize firstFramePosition if fit isn't
+                    if context.scene.kmc_props.firstFramePosition[1] == -1:
+                        context.scene.kmc_props.firstFramePosition = (-1.0*head[X], head[Y], head[Z])
+
+                    ffp = context.scene.kmc_props.firstFramePosition
+                    tx = context.scene.kmc_props.initialOffset[0]
+                    ty = context.scene.kmc_props.initialOffset[2]
+                    tz = context.scene.kmc_props.initialOffset[1]
+                    if not context.scene.kmc_props.lockwidth:
+                        tx += -head[X] - ffp[0]
+                    if not context.scene.kmc_props.lockHeight:
+                        ty += head[Z] - ffp[2]
+                    if not context.scene.kmc_props.lockDepth:
+                        tz += head[Y] - ffp[1]
+
+                    # translate bone
+                    bone.matrix.translation = (tx, tz, ty)
+
                 # convert rotation in local coordinates
                 boneV = boneV * bone.matrix
 
@@ -256,6 +289,15 @@ def draw(self, context):
                         box.prop(target, "value", text=target.name)
                         break
 
+            # configure movement tracking
+            layout.separator()
+            box = layout.box()
+            box.label(text="Lock movement for :")
+            col = box.column_flow(columns=3)
+            col.prop(context.scene.kmc_props, "lockDepth")
+            col.prop(context.scene.kmc_props, "lockHeight")
+            col.prop(context.scene.kmc_props, "lockwidth")
+
             # activate
             layout.separator()
             layout.operator("kmc.start")

scripts/2.80/kinect_mocap.py

@@ -23,7 +23,7 @@
     "name": "Kinect Motion Capture plugin",
     "description": "Motion capture using MS Kinect v2",
     "author": "Morgane Dufresne",
-    "version": (1, 2),
+    "version": (1, 3),
     "blender": (2, 80, 0),
     "warning": "You need a MS Kinect v2 sensor (XBox One)",
     "support": "COMMUNITY",
@@ -70,6 +70,11 @@ class KMC_PG_KmcProperties(bpy.types.PropertyGroup):
     record : bpy.props.BoolProperty(name="Record captured motion", description="activate recording while tracking")
     isTracking : bpy.props.BoolProperty(name="Tracking status", description="tracking status")
     stopTracking : bpy.props.BoolProperty(name="Stop trigger", description="tells to stop the tracking")
+    firstFramePosition : bpy.props.FloatVectorProperty(name="firstFramePosition", description="position of root bone in first frame", size=3)
+    initialOffset : bpy.props.FloatVectorProperty(name="initialOffset", description="position of root bone in rest pose", size=3)
+    lockHeight : bpy.props.BoolProperty(name="height", description="ignore vertical movement", default=True)
+    lockwidth : bpy.props.BoolProperty(name="width", description="ignore lateral movement", default=True)
+    lockDepth : bpy.props.BoolProperty(name="depth", description="ignore depth movement", default=True)
 
 jointType = {
     "SpineBase":0,
@@ -171,12 +176,18 @@ def initialize(context):
     bpy.ops.pose.select_all(action=('DESELECT'))
     context.scene.kmc_props.currentFrame = 0
     context.scene.kmc_props.stopTracking = False
+    context.scene.kmc_props.firstFramePosition = (-1,-1,-1)
+    context.scene.kmc_props.initialOffset = (0,0,0)
 
     for target in context.scene.kmc_props.targetBones:
         if target.value is not None and target.value != "" :
             bone = bpy.data.objects[context.scene.kmc_props.arma_list].pose.bones[target.value]
             bone.rotation_mode = 'QUATERNION'
 
+            # Store initial position of spine0 bone
+            if target.name == "Spine0":
+                context.scene.kmc_props.initialOffset = bpy.data.objects[context.scene.kmc_props.arma_list].pose.bones[target.value].matrix.translation
+
             # Store rest pose angles for column, head and feet bones
             if bonesDefinition[target.name][2] is not None :
                 baseDir =  bonesDefinition[target.name][2] @ bone.matrix
@@ -200,7 +211,27 @@ def updatePose(context, bone):
             # update only tracked bones
             if(head[3] == 2) and (tail[3] == 2) :
                 boneV = Vector((head[X] - tail[X], tail[Y] - head[Y], tail[Z] - head[Z]))
-
+
+                # if first bone, update position (only for configured axes)
+                if target.name == "Spine0":
+                    # initialize firstFramePosition if fit isn't
+                    if context.scene.kmc_props.firstFramePosition[1] == -1:
+                        context.scene.kmc_props.firstFramePosition = (-1.0*head[X], head[Y], head[Z])
+
+                    ffp = context.scene.kmc_props.firstFramePosition
+                    tx = context.scene.kmc_props.initialOffset[0]
+                    ty = context.scene.kmc_props.initialOffset[2]
+                    tz = context.scene.kmc_props.initialOffset[1]
+                    if not context.scene.kmc_props.lockwidth:
+                        tx += -head[X] - ffp[0]
+                    if not context.scene.kmc_props.lockHeight:
+                        ty += head[Z] - ffp[2]
+                    if not context.scene.kmc_props.lockDepth:
+                        tz += head[Y] - ffp[1]
+
+                    # translate bone
+                    bone.matrix.translation = (tx, tz, ty)
+
                 # convert rotation in local coordinates
                 boneV = boneV @ bone.matrix
 
@@ -262,6 +293,15 @@ def draw(self, context):
                         box.prop(target, "value", text=target.name)
                         break
 
+            # configure movement tracking
+            layout.separator()
+            box = layout.box()
+            box.label(text="Lock movement for :")
+            col = box.column_flow(columns=3)
+            col.prop(context.scene.kmc_props, "lockDepth")
+            col.prop(context.scene.kmc_props, "lockHeight")
+            col.prop(context.scene.kmc_props, "lockwidth")
+
             # activate
             layout.separator()
             layout.operator("kmc.start")
@@ -303,7 +343,6 @@ def captureFrame(context):
     if(context.scene.k_sensor.update() == 1):
         # update pose
         updatePose(context, bpy.data.objects[context.scene.kmc_props.arma_list].pose.bones[0])
-        #context.scene.update()
 
     if context.scene.kmc_props.currentFrame > 0 :
         context.scene.kmc_props.currentFrame += 1
@@ -329,7 +368,7 @@ def execute(self, context):
         else:
             # init system
             initialize(context)
-
+        
             context.scene.k_sensor.init(1.0 / context.scene.kmc_props.fps)
             bpy.app.timers.register(functools.partial(captureFrame, context))
             context.scene.kmc_props.isTracking = True