From a6f613adcfc13bee3c9da850c035a21e317e1d10 Mon Sep 17 00:00:00 2001
From: Moraell <morgane.dufr@gmail.com>
Date: Sat, 20 Jul 2019 13:11:31 +0200
Subject: [PATCH] Version 1.2 : tilt compensation

---
 CHANGELOG.md                 |  4 ++++
 kinectMocap.cpp              | 30 +++++++++++++++++++++++++++++-
 kinectMocap.h                |  1 +
 scripts/2.80/kinect_mocap.py |  6 +++---
 4 files changed, 37 insertions(+), 4 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 8d8b32f..7c1f90b 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,3 +1,7 @@
+Version 1.2:
+- added smoothing with Kalman filter
+- added sensor tilt angle compensation
+
 Version 1.1:
 - added Blender 2.80 version
 - moved python scripts to separate directories in order to allow different versions of Blender
diff --git a/kinectMocap.cpp b/kinectMocap.cpp
index 2594f91..b09f90b 100644
--- a/kinectMocap.cpp
+++ b/kinectMocap.cpp
@@ -20,6 +20,7 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
 */
 #include "kinectMocap.h"
+#include <cmath>
 
 // Safe release for interfaces
 template<class Interface>
@@ -35,6 +36,8 @@ inline void SafeRelease(Interface *& pInterfaceToRelease)
 int initSensor(double inDt) {
 	HRESULT hr;
 	int res = 0;
+	tilt = -100;
+	sensorH = -1;
 	dt = inDt;
 
 	hr = GetDefaultKinectSensor(&m_pKinectSensor);
@@ -127,6 +130,18 @@ int updateFrame() {
 	
 	if (SUCCEEDED(hr)) {
 
+		if (tilt == -100) {
+			// initialize tilt angle
+			Vector4 floorPlane;
+
+			if (SUCCEEDED(pBodyFrame->get_FloorClipPlane(&floorPlane))) {
+				tilt = atan2(floorPlane.z, floorPlane.y);
+				sensorH = floorPlane.w;
+				planeX = floorPlane.x;
+				planeY = floorPlane.y;
+				planeZ = floorPlane.z;
+			}
+		}
 		
 		IBody* ppBodies[BODY_COUNT] = { 0 };
 
@@ -148,8 +163,16 @@ int updateFrame() {
 						hr = pBody->GetJoints(_countof(joints), joints);
 						
 						if (SUCCEEDED(hr)) {
-							// apply kalman filter to each joint
 							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;
+								}
+
+								// apply kalman filter to each joint
 								applyKalman(j);
 							}
 							res = 1;
@@ -166,12 +189,16 @@ 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) {
@@ -180,5 +207,6 @@ BOOST_PYTHON_MODULE(kinectMocap4Blender) {
 		.def("close", &Sensor::close)
 		.def("update", &Sensor::update)
 		.def("getJoint", &Sensor::getJoint)
+		.def("tilt", &Sensor::tilt)
 	;
 }
\ No newline at end of file
diff --git a/kinectMocap.h b/kinectMocap.h
index 1b3a228..1cc889d 100644
--- a/kinectMocap.h
+++ b/kinectMocap.h
@@ -31,6 +31,7 @@ using namespace boost::python;
 
 // Current Kinect
 IKinectSensor*          m_pKinectSensor;
+double					tilt, sensorH, planeX, planeY, planeZ;
 
 Joint					joints[JointType_Count];
 
diff --git a/scripts/2.80/kinect_mocap.py b/scripts/2.80/kinect_mocap.py
index 5ba111a..7905372 100644
--- a/scripts/2.80/kinect_mocap.py
+++ b/scripts/2.80/kinect_mocap.py
@@ -171,12 +171,12 @@ def initialize(context):
     bpy.ops.pose.select_all(action=('DESELECT'))
     context.scene.kmc_props.currentFrame = 0
     context.scene.kmc_props.stopTracking = False
-
+    
     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 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 +200,7 @@ 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]))
-                
+            
                 # convert rotation in local coordinates
                 boneV = boneV @ bone.matrix