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 // Safe release for interfaces template @@ -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(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