From ff8b68a0e0fde300a677bf55ddd156882a4a158d Mon Sep 17 00:00:00 2001 From: Nanospork Date: Fri, 4 Aug 2017 01:38:41 -0700 Subject: [PATCH 01/19] Filter successfully implemented, but with rotation unity artifacts. Need to fix the -180<->+180 problem. --- FusionMath.cpp | 11 +++++++ FusionMath.h | 1 + OrientationReader.cpp | 69 ++++++++++++++++++++++++++++++++++++++++++- OrientationReader.h | 12 ++++++++ 4 files changed, 92 insertions(+), 1 deletion(-) diff --git a/FusionMath.cpp b/FusionMath.cpp index 3bb006e..e14b3bc 100644 --- a/FusionMath.cpp +++ b/FusionMath.cpp @@ -26,4 +26,15 @@ namespace je_nourish_fusion { osvrQuatSetW(quaternion, cos(r / 2) * cos(p / 2) * cos(y / 2) + sin(r / 2) * sin(p / 2) * sin(y / 2)); } + double fixUnityAngle(double angle) + { + if (angle > M_PI * 17/16) { + angle = (2 * M_PI) - angle; + } + else if (angle < -M_PI * 17/16) { + angle = (2 * M_PI) + angle; + } + return angle; + } + } \ No newline at end of file diff --git a/FusionMath.h b/FusionMath.h index 27da552..36a6892 100644 --- a/FusionMath.h +++ b/FusionMath.h @@ -4,5 +4,6 @@ namespace je_nourish_fusion { void rpyFromQuaternion(OSVR_Quaternion* quaternion, OSVR_Vec3* rpy); void quaternionFromRPY(OSVR_Vec3* rpy, OSVR_Quaternion* quaternion); + double fixUnityAngle(double angle); } \ No newline at end of file diff --git a/OrientationReader.cpp b/OrientationReader.cpp index be3eb3d..d2d1f54 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -9,7 +9,11 @@ namespace je_nourish_fusion { if (config.isString()) { reader = new SingleOrientationReader(ctx, config.asString()); } - if (config.isObject() && config.isMember("roll") && config.isMember("pitch") && config.isMember("yaw")) { + else if (config.isObject() && config.isMember("roll") && config.isMember("pitch") && config.isMember("yawFast") + && config.isMember("yawAccurate") && config.isMember("alpha")) { + reader = new FilteredOrientationReader(ctx, config); + } + else if (config.isObject() && config.isMember("roll") && config.isMember("pitch") && config.isMember("yaw")) { reader = new CombinedOrientationReader(ctx, config); } @@ -29,6 +33,15 @@ namespace je_nourish_fusion { osvrClientGetInterface(ctx, orientation_paths["yaw"].asCString(), &(m_orientations[2])); } + FilteredOrientationReader::FilteredOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths) { + osvrClientGetInterface(ctx, orientation_paths["roll"].asCString(), &(m_orientations[0])); + osvrClientGetInterface(ctx, orientation_paths["pitch"].asCString(), &(m_orientations[1])); + osvrClientGetInterface(ctx, orientation_paths["yawFast"].asCString(), &(m_orientations[2])); + osvrClientGetInterface(ctx, orientation_paths["yawAccurate"].asCString(), &(m_orientations[3])); + // TO DO: Is this a bogus way to read in this value? Do I care? + m_alpha = orientation_paths["alpha"].asDouble(); + } + OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) { OSVR_OrientationState orientation_x; OSVR_OrientationState orientation_y; @@ -56,4 +69,58 @@ namespace je_nourish_fusion { return OSVR_RETURN_SUCCESS; } + OSVR_ReturnCode FilteredOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) { + OSVR_OrientationState orientation_x; + OSVR_OrientationState orientation_y; + OSVR_OrientationState orientation_z; + OSVR_OrientationState orientation_z2; + + OSVR_ReturnCode xret = osvrGetOrientationState(m_orientations[0], timeValue, &orientation_x); + OSVR_ReturnCode yret = osvrGetOrientationState(m_orientations[1], timeValue, &orientation_y); + OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[2], timeValue, &orientation_z); + OSVR_ReturnCode z2ret = osvrGetOrientationState(m_orientations[3], timeValue, &orientation_z2); + + OSVR_Vec3 rpy_x; + OSVR_Vec3 rpy_y; + OSVR_Vec3 rpy_z; + OSVR_Vec3 rpy_z2; + + rpyFromQuaternion(&orientation_x, &rpy_x); + rpyFromQuaternion(&orientation_y, &rpy_y); + rpyFromQuaternion(&orientation_z, &rpy_z); + rpyFromQuaternion(&orientation_z2, &rpy_z2); + + /*--------------------------------------- + // Filter Implementation Here + ---------------------------------------*/ + double z_fast; + double z_accurate; + double z_out; + double a = m_alpha; + double last_y = m_last_yaw; + double dt = osvr::util::time::duration(*timeValue, *m_last_timeValue); + + z_fast = osvrVec3GetZ(&rpy_z); + z_accurate = osvrVec3GetZ(&rpy_z2); + + //z_fast = fixUnityAngle(z_fast); + //z_accurate = fixUnityAngle(z_accurate); + + z_out = a*(last_y + z_fast*dt) + (1 - a)*(z_accurate); + + z_out = fixUnityAngle(z_out); + + m_last_yaw = z_out; + m_last_timeValue = timeValue; + + OSVR_Vec3 rpy; + osvrVec3SetX(&rpy, osvrVec3GetX(&rpy_x)); + osvrVec3SetY(&rpy, osvrVec3GetY(&rpy_y)); + osvrVec3SetZ(&rpy, z_out); + + quaternionFromRPY(&rpy, orientation); + + return OSVR_RETURN_SUCCESS; + } + } \ No newline at end of file diff --git a/OrientationReader.h b/OrientationReader.h index 7267845..2405ab2 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -1,4 +1,5 @@ #include "stdafx.h" +#include namespace je_nourish_fusion { @@ -28,4 +29,15 @@ namespace je_nourish_fusion { OSVR_ClientInterface m_orientations[3]; }; + class FilteredOrientationReader : public IOrientationReader { + public: + FilteredOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths); + OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue); + protected: + OSVR_ClientInterface m_orientations[4]; + double m_alpha; + double m_last_yaw; + OSVR_TimeValue* m_last_timeValue; + }; + } \ No newline at end of file From 057fb921d74b8f2592785dd88154c12e33b159d1 Mon Sep 17 00:00:00 2001 From: Nanospork Date: Fri, 4 Aug 2017 20:43:00 -0700 Subject: [PATCH 02/19] Filter progress. --- OrientationReader.cpp | 32 +++++++++++++++++++++++++------- OrientationReader.h | 4 +++- 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/OrientationReader.cpp b/OrientationReader.cpp index d2d1f54..7b4cd83 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -33,13 +33,16 @@ namespace je_nourish_fusion { osvrClientGetInterface(ctx, orientation_paths["yaw"].asCString(), &(m_orientations[2])); } - FilteredOrientationReader::FilteredOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths) { + FilteredOrientationReader::FilteredOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths) : m_ctx(ctx) { osvrClientGetInterface(ctx, orientation_paths["roll"].asCString(), &(m_orientations[0])); osvrClientGetInterface(ctx, orientation_paths["pitch"].asCString(), &(m_orientations[1])); osvrClientGetInterface(ctx, orientation_paths["yawFast"].asCString(), &(m_orientations[2])); osvrClientGetInterface(ctx, orientation_paths["yawAccurate"].asCString(), &(m_orientations[3])); // TO DO: Is this a bogus way to read in this value? Do I care? m_alpha = orientation_paths["alpha"].asDouble(); + m_last_yaw = 0; + m_last_zfast = 0; + m_last_timeValue = osvr::util::time::getNow(); } OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) { @@ -97,21 +100,36 @@ namespace je_nourish_fusion { double z_accurate; double z_out; double a = m_alpha; - double last_y = m_last_yaw; - double dt = osvr::util::time::duration(*timeValue, *m_last_timeValue); + double last_z = m_last_yaw; + double last_z_fast = m_last_zfast; + double dt = ::osvr::util::time::duration(*timeValue, m_last_timeValue); + m_last_timeValue = *timeValue; z_fast = osvrVec3GetZ(&rpy_z); z_accurate = osvrVec3GetZ(&rpy_z2); - //z_fast = fixUnityAngle(z_fast); - //z_accurate = fixUnityAngle(z_accurate); + z_fast = fixUnityAngle(z_fast); + z_accurate = fixUnityAngle(z_accurate); - z_out = a*(last_y + z_fast*dt) + (1 - a)*(z_accurate); + if ((z_accurate < -M_PI / 2 && last_z > M_PI / 2) || (z_accurate > M_PI / 2 && last_z < -M_PI / 2)) { + last_z = z_accurate; + //z_fast = z_accurate; + } + + z_out = a*(last_z + (z_fast-last_z_fast)*dt) + (1 - a)*(z_accurate); + + // TO-DO: Eventually this can be removed. It should be unnecessary. + if (std::isnan(z_out)) { + z_out = 0; + } + + //m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "data:"); + //m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(last_z).c_str()); z_out = fixUnityAngle(z_out); m_last_yaw = z_out; - m_last_timeValue = timeValue; + m_last_zfast = z_fast; OSVR_Vec3 rpy; osvrVec3SetX(&rpy, osvrVec3GetX(&rpy_x)); diff --git a/OrientationReader.h b/OrientationReader.h index 2405ab2..961a79c 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -35,9 +35,11 @@ namespace je_nourish_fusion { OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue); protected: OSVR_ClientInterface m_orientations[4]; + osvr::clientkit::ClientContext m_ctx; double m_alpha; double m_last_yaw; - OSVR_TimeValue* m_last_timeValue; + double m_last_zfast; + OSVR_TimeValue m_last_timeValue; }; } \ No newline at end of file From 459fc2d969ae4c6d76e05e49a3acffaec3e84b21 Mon Sep 17 00:00:00 2001 From: Nanospork Date: Fri, 4 Aug 2017 23:33:36 -0700 Subject: [PATCH 03/19] Filter now fully functional, but source code is still dirty. High-pass component now works properly using angularVelocity and angle wrapping works as well. --- OrientationReader.cpp | 41 ++++++++++++++++++++++++++++------------- OrientationReader.h | 1 - 2 files changed, 28 insertions(+), 14 deletions(-) diff --git a/OrientationReader.cpp b/OrientationReader.cpp index 7b4cd83..ce67485 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -41,7 +41,6 @@ namespace je_nourish_fusion { // TO DO: Is this a bogus way to read in this value? Do I care? m_alpha = orientation_paths["alpha"].asDouble(); m_last_yaw = 0; - m_last_zfast = 0; m_last_timeValue = osvr::util::time::getNow(); } @@ -54,6 +53,7 @@ namespace je_nourish_fusion { OSVR_ReturnCode yret = osvrGetOrientationState(m_orientations[1], timeValue, &orientation_y); OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[2], timeValue, &orientation_z); + OSVR_Vec3 rpy_x; OSVR_Vec3 rpy_y; OSVR_Vec3 rpy_z; @@ -76,37 +76,43 @@ namespace je_nourish_fusion { OSVR_OrientationState orientation_x; OSVR_OrientationState orientation_y; OSVR_OrientationState orientation_z; - OSVR_OrientationState orientation_z2; + OSVR_AngularVelocityState angular_v; OSVR_ReturnCode xret = osvrGetOrientationState(m_orientations[0], timeValue, &orientation_x); OSVR_ReturnCode yret = osvrGetOrientationState(m_orientations[1], timeValue, &orientation_y); - OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[2], timeValue, &orientation_z); - OSVR_ReturnCode z2ret = osvrGetOrientationState(m_orientations[3], timeValue, &orientation_z2); + OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[3], timeValue, &orientation_z); + OSVR_ReturnCode angret = osvrGetAngularVelocityState(m_orientations[2], timeValue, &angular_v); OSVR_Vec3 rpy_x; OSVR_Vec3 rpy_y; OSVR_Vec3 rpy_z; - OSVR_Vec3 rpy_z2; + OSVR_Vec3 rpy_v; rpyFromQuaternion(&orientation_x, &rpy_x); rpyFromQuaternion(&orientation_y, &rpy_y); rpyFromQuaternion(&orientation_z, &rpy_z); - rpyFromQuaternion(&orientation_z2, &rpy_z2); + rpyFromQuaternion(&angular_v.incrementalRotation, &rpy_v); /*--------------------------------------- // Filter Implementation Here ---------------------------------------*/ + double dz_fast; double z_fast; double z_accurate; + double dt; double z_out; double a = m_alpha; double last_z = m_last_yaw; - double last_z_fast = m_last_zfast; - double dt = ::osvr::util::time::duration(*timeValue, m_last_timeValue); - m_last_timeValue = *timeValue; + + dt = angular_v.dt * 2 * M_PI; + + //dt = ::osvr::util::time::duration(*timeValue, m_last_timeValue); + //m_last_timeValue = *timeValue; - z_fast = osvrVec3GetZ(&rpy_z); - z_accurate = osvrVec3GetZ(&rpy_z2); + dz_fast = osvrVec3GetZ(&rpy_v); + z_accurate = osvrVec3GetZ(&rpy_z); + + z_fast = dt * dz_fast; z_fast = fixUnityAngle(z_fast); z_accurate = fixUnityAngle(z_accurate); @@ -116,20 +122,29 @@ namespace je_nourish_fusion { //z_fast = z_accurate; } - z_out = a*(last_z + (z_fast-last_z_fast)*dt) + (1 - a)*(z_accurate); + z_out = a*(last_z + z_fast) + (1 - a)*(z_accurate); // TO-DO: Eventually this can be removed. It should be unnecessary. if (std::isnan(z_out)) { z_out = 0; } + static int boop; + boop += 1; + if (boop >= 250) + { + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "dt"); + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(dt).c_str()); + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(z_out).c_str()); + boop = 0; + } + //m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "data:"); //m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(last_z).c_str()); z_out = fixUnityAngle(z_out); m_last_yaw = z_out; - m_last_zfast = z_fast; OSVR_Vec3 rpy; osvrVec3SetX(&rpy, osvrVec3GetX(&rpy_x)); diff --git a/OrientationReader.h b/OrientationReader.h index 961a79c..5f03b5e 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -38,7 +38,6 @@ namespace je_nourish_fusion { osvr::clientkit::ClientContext m_ctx; double m_alpha; double m_last_yaw; - double m_last_zfast; OSVR_TimeValue m_last_timeValue; }; From d6607c08bbe587bf2e843f5694c719d27a9416fb Mon Sep 17 00:00:00 2001 From: Nanospork Date: Sat, 5 Aug 2017 01:59:43 -0700 Subject: [PATCH 04/19] Code cleanup. --- OrientationReader.cpp | 47 +++++++++++-------------------------------- OrientationReader.h | 1 - 2 files changed, 12 insertions(+), 36 deletions(-) diff --git a/OrientationReader.cpp b/OrientationReader.cpp index ce67485..57d9219 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -38,10 +38,10 @@ namespace je_nourish_fusion { osvrClientGetInterface(ctx, orientation_paths["pitch"].asCString(), &(m_orientations[1])); osvrClientGetInterface(ctx, orientation_paths["yawFast"].asCString(), &(m_orientations[2])); osvrClientGetInterface(ctx, orientation_paths["yawAccurate"].asCString(), &(m_orientations[3])); - // TO DO: Is this a bogus way to read in this value? Do I care? m_alpha = orientation_paths["alpha"].asDouble(); m_last_yaw = 0; - m_last_timeValue = osvr::util::time::getNow(); + + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Initialized a complementary fusion filter."); } OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) { @@ -93,54 +93,31 @@ namespace je_nourish_fusion { rpyFromQuaternion(&orientation_z, &rpy_z); rpyFromQuaternion(&angular_v.incrementalRotation, &rpy_v); - /*--------------------------------------- - // Filter Implementation Here - ---------------------------------------*/ - double dz_fast; - double z_fast; - double z_accurate; - double dt; - double z_out; double a = m_alpha; double last_z = m_last_yaw; - dt = angular_v.dt * 2 * M_PI; - - //dt = ::osvr::util::time::duration(*timeValue, m_last_timeValue); - //m_last_timeValue = *timeValue; + // A factor of 2*PI is missing in angularVelocity incremental quats - at least with the HDK. + double dt = angular_v.dt * 2 * M_PI; - dz_fast = osvrVec3GetZ(&rpy_v); - z_accurate = osvrVec3GetZ(&rpy_z); + double z_accurate = osvrVec3GetZ(&rpy_z); + double dzdt_fast = osvrVec3GetZ(&rpy_v); - z_fast = dt * dz_fast; + double dz_fast = dt * dzdt_fast; - z_fast = fixUnityAngle(z_fast); + dz_fast = fixUnityAngle(dz_fast); z_accurate = fixUnityAngle(z_accurate); + // Correct for the singularity at +-180 degrees if ((z_accurate < -M_PI / 2 && last_z > M_PI / 2) || (z_accurate > M_PI / 2 && last_z < -M_PI / 2)) { last_z = z_accurate; - //z_fast = z_accurate; } - z_out = a*(last_z + z_fast) + (1 - a)*(z_accurate); + double z_out = a*(last_z + dz_fast) + (1 - a)*(z_accurate); - // TO-DO: Eventually this can be removed. It should be unnecessary. + // Replace bogus results with accurate yaw. Happens sometimes on startup. if (std::isnan(z_out)) { - z_out = 0; + z_out = z_accurate; } - - static int boop; - boop += 1; - if (boop >= 250) - { - m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "dt"); - m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(dt).c_str()); - m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(z_out).c_str()); - boop = 0; - } - - //m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "data:"); - //m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(last_z).c_str()); z_out = fixUnityAngle(z_out); diff --git a/OrientationReader.h b/OrientationReader.h index 5f03b5e..24413fe 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -38,7 +38,6 @@ namespace je_nourish_fusion { osvr::clientkit::ClientContext m_ctx; double m_alpha; double m_last_yaw; - OSVR_TimeValue m_last_timeValue; }; } \ No newline at end of file From 5ee6a1ad5f210e7daade174ac30e8212baa7f500 Mon Sep 17 00:00:00 2001 From: Nanospork Date: Sat, 5 Aug 2017 12:22:30 -0700 Subject: [PATCH 05/19] Updated README.md: added information and example for complementary filter. --- README.md | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 65fc1fc..852a6b1 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,8 @@ An OSVR plugin that creates trackers from different sources of data. For example It can also combine axes from different trackers, eg taking pitch and roll from an accelerometer and yaw from a magnetometer, or x and y position from a video tracker and z position from a depth camera. +A complementary filter can be used to smoothly merge yaw data from one device with another. For example, a faster gyroscopic yaw reading from one device can be combined with a more accurate video tracker yaw reading from another device. + Build following the [standard OSVR plugin build instructions](http://resource.osvr.com/docs/OSVR-Core/TopicWritingDevicePlugin.html). ## Tracker alignment @@ -51,10 +53,31 @@ This replaces the `alignInitialOrientation` option in previous versions. "yaw": "/je_nourish_kinectv2/KinectV2/semantic/body1/arms/right/hand" } } + }, + // Use the complementary filter to improve the yaw performance of a different Wii Nunchuk + { + "plugin": "je_nourish_fusion", + "driver": "FusionDevice", + "params": { + "name": "Wii_Kinect_Left", + "position": "/je_nourish_kinect/KinectV2/semantic/body1/arms/left/hand", + "orientation": { + "roll": "/je_nourish_wiimote/WiimoteDevice/semantic/wiimote2/nunchuk", + "pitch": "/je_nourish_wiimote/WiimoteDevice/semantic/wiimote2/nunchuk", + // Nunchuk yaw updates faster, but Kinect yaw is more accurate over time (doesn't drift) + "yawFast": "/je_nourish_wiimote/WiimoteDevice/semantic/wiimote2/nunchuk", + "yawAccurate": "/je_nourish_kinectv2/KinectV2/semantic/body1/arms/left/hand", + // Alpha must be between 0 and 1. Usually > 0.95 + "alpha": 0.99 + }, + // Pass the faster timestamp from the Nunchuk to OSVR. + "timestamp": "rotation" + } } ], "aliases": { "/me/head": "/je_nourish_fusion/DK1_Kinectv2/tracker/0", - "/me/hands/right": "/je_nourish_fusion/Wii_Kinect_Right/tracker/0" + "/me/hands/right": "/je_nourish_fusion/Wii_Kinect_Right/tracker/0", + "/me/hands/left": "/je_nourish_fusion/Wii_Kinect_Left/tracker/0" } - } + } \ No newline at end of file From b6418d1c89dc7c3566e4ac62003503cb2208f636 Mon Sep 17 00:00:00 2001 From: Nanospork Date: Sun, 6 Aug 2017 00:38:45 -0700 Subject: [PATCH 06/19] Updated README.md to correct misleading information. --- README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 852a6b1..c217ba8 100644 --- a/README.md +++ b/README.md @@ -54,23 +54,23 @@ This replaces the `alignInitialOrientation` option in previous versions. } } }, - // Use the complementary filter to improve the yaw performance of a different Wii Nunchuk + // Use the complementary filter to improve the yaw performance of a home-made gyro device { "plugin": "je_nourish_fusion", "driver": "FusionDevice", "params": { - "name": "Wii_Kinect_Left", + "name": "Gyro_Kinect_Left", "position": "/je_nourish_kinect/KinectV2/semantic/body1/arms/left/hand", "orientation": { - "roll": "/je_nourish_wiimote/WiimoteDevice/semantic/wiimote2/nunchuk", - "pitch": "/je_nourish_wiimote/WiimoteDevice/semantic/wiimote2/nunchuk", - // Nunchuk yaw updates faster, but Kinect yaw is more accurate over time (doesn't drift) - "yawFast": "/je_nourish_wiimote/WiimoteDevice/semantic/wiimote2/nunchuk", + "roll": "/my-gyro-device/semantic/controller/left", + "pitch": "/my-gyro-device/semantic/controller/left", + // Gyro yaw updates faster, but Kinect yaw is more accurate over time (doesn't drift) + "yawFast": "/my-gyro-device/semantic/controller/left", "yawAccurate": "/je_nourish_kinectv2/KinectV2/semantic/body1/arms/left/hand", - // Alpha must be between 0 and 1. Usually > 0.95 + // Alpha must be between 0 and 1. Usually > 0.95, favoring speed. "alpha": 0.99 }, - // Pass the faster timestamp from the Nunchuk to OSVR. + // Pass the faster timestamp from the gyro device to OSVR. "timestamp": "rotation" } } @@ -78,6 +78,6 @@ This replaces the `alignInitialOrientation` option in previous versions. "aliases": { "/me/head": "/je_nourish_fusion/DK1_Kinectv2/tracker/0", "/me/hands/right": "/je_nourish_fusion/Wii_Kinect_Right/tracker/0", - "/me/hands/left": "/je_nourish_fusion/Wii_Kinect_Left/tracker/0" + "/me/hands/left": "/je_nourish_fusion/Gyro_Kinect_Left/tracker/0" } } \ No newline at end of file From f1f4668114b959482a7a32efed031ca44b8b0710 Mon Sep 17 00:00:00 2001 From: Nanospork Date: Mon, 7 Aug 2017 22:40:34 -0700 Subject: [PATCH 07/19] Removed unnecessary chrono include. --- OrientationReader.h | 1 - 1 file changed, 1 deletion(-) diff --git a/OrientationReader.h b/OrientationReader.h index 24413fe..8f4ccd4 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -1,5 +1,4 @@ #include "stdafx.h" -#include namespace je_nourish_fusion { From 78cda4e7d331f98fccb09d511399c825aa2e51f2 Mon Sep 17 00:00:00 2001 From: Nanospork Date: Mon, 7 Aug 2017 22:43:14 -0700 Subject: [PATCH 08/19] Removed the 'fudging factor' from the wraparound code. May be more precise this way. --- FusionMath.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/FusionMath.cpp b/FusionMath.cpp index e14b3bc..faa58ed 100644 --- a/FusionMath.cpp +++ b/FusionMath.cpp @@ -28,10 +28,10 @@ namespace je_nourish_fusion { double fixUnityAngle(double angle) { - if (angle > M_PI * 17/16) { + if (angle > M_PI) { angle = (2 * M_PI) - angle; } - else if (angle < -M_PI * 17/16) { + else if (angle < -M_PI) { angle = (2 * M_PI) + angle; } return angle; From 93bdd4d34c540ac5410264d03e8c6c6bc626bb61 Mon Sep 17 00:00:00 2001 From: Nanospork Date: Fri, 11 Aug 2017 16:38:02 -0700 Subject: [PATCH 09/19] Added workaroud for 'smooth' yaw resets. May need future work. --- OrientationReader.cpp | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/OrientationReader.cpp b/OrientationReader.cpp index 57d9219..44234a6 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -95,7 +95,7 @@ namespace je_nourish_fusion { double a = m_alpha; double last_z = m_last_yaw; - + // A factor of 2*PI is missing in angularVelocity incremental quats - at least with the HDK. double dt = angular_v.dt * 2 * M_PI; @@ -112,8 +112,22 @@ namespace je_nourish_fusion { last_z = z_accurate; } - double z_out = a*(last_z + dz_fast) + (1 - a)*(z_accurate); - + double z_diff = fixUnityAngle(last_z - z_accurate); + double z_out; + + // If the yaw difference is large enough > 10 deg but the rotation rate is small enough (< 2 deg/sec) + // then reset filter output to the accurate yaw value. + // This fixes the issue of "smooth" external yaw resets, making them instantaneous. + // Cutoff values determined empirically; in the future, perhaps these values could be configurable. + if ((z_diff <= -M_PI / 18 || z_diff >= M_PI / 18) && (dzdt_fast <= M_PI / 90 && dzdt_fast >= -M_PI / 90)) { + z_out = z_accurate; + } + // If the difference is smaller, implement the complementary filter. + else { + z_out = a*(last_z + dz_fast) + (1 - a)*(z_accurate); + } + + // Replace bogus results with accurate yaw. Happens sometimes on startup. if (std::isnan(z_out)) { z_out = z_accurate; From 3255ba199342744f37a0c8a1d7199f3369347112 Mon Sep 17 00:00:00 2001 From: Nanospork Date: Fri, 11 Aug 2017 20:00:11 -0700 Subject: [PATCH 10/19] Added 'instantReset' parameter for instantReset, off by default for now. Tweaked yaw reset trigger settings. --- OrientationReader.cpp | 16 ++++++++++++++-- OrientationReader.h | 1 + 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/OrientationReader.cpp b/OrientationReader.cpp index 44234a6..066f350 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -39,6 +39,16 @@ namespace je_nourish_fusion { osvrClientGetInterface(ctx, orientation_paths["yawFast"].asCString(), &(m_orientations[2])); osvrClientGetInterface(ctx, orientation_paths["yawAccurate"].asCString(), &(m_orientations[3])); m_alpha = orientation_paths["alpha"].asDouble(); + if (orientation_paths["instantReset"].isNull()) { + m_instantReset = false; + } else { + m_instantReset = orientation_paths["instantReset"].asBool(); + if (m_instantReset) { + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "instantReset is enabled in OSVR-Fusion."); + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "If you notice stuttering, disable instantReset."); + } + } + m_last_yaw = 0; m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Initialized a complementary fusion filter."); @@ -115,11 +125,12 @@ namespace je_nourish_fusion { double z_diff = fixUnityAngle(last_z - z_accurate); double z_out; - // If the yaw difference is large enough > 10 deg but the rotation rate is small enough (< 2 deg/sec) + // If the yaw difference is large enough > 30 deg but the rotation rate is small enough (< 2 deg/sec) // then reset filter output to the accurate yaw value. // This fixes the issue of "smooth" external yaw resets, making them instantaneous. // Cutoff values determined empirically; in the future, perhaps these values could be configurable. - if ((z_diff <= -M_PI / 18 || z_diff >= M_PI / 18) && (dzdt_fast <= M_PI / 90 && dzdt_fast >= -M_PI / 90)) { + if (m_instantReset && + (z_diff < -M_PI / 6 || z_diff > M_PI / 6) && (dzdt_fast < M_PI / 90 && dzdt_fast > -M_PI / 90)) { z_out = z_accurate; } // If the difference is smaller, implement the complementary filter. @@ -133,6 +144,7 @@ namespace je_nourish_fusion { z_out = z_accurate; } + // Keep the filter output nice and clean z_out = fixUnityAngle(z_out); m_last_yaw = z_out; diff --git a/OrientationReader.h b/OrientationReader.h index 8f4ccd4..a73ea80 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -35,6 +35,7 @@ namespace je_nourish_fusion { protected: OSVR_ClientInterface m_orientations[4]; osvr::clientkit::ClientContext m_ctx; + bool m_instantReset; double m_alpha; double m_last_yaw; }; From e1e96e44c95768f2fe0e8fc03b4726b102d1a02a Mon Sep 17 00:00:00 2001 From: Nanospork Date: Tue, 15 Aug 2017 13:22:23 -0700 Subject: [PATCH 11/19] Added more stringent motion checks and button check to instant yaw reset. Now takes a path instead of a boolean. --- OrientationReader.cpp | 47 ++++++++++++++++++++++++++++++++----------- OrientationReader.h | 4 +++- 2 files changed, 38 insertions(+), 13 deletions(-) diff --git a/OrientationReader.cpp b/OrientationReader.cpp index 066f350..c619e2d 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -40,13 +40,12 @@ namespace je_nourish_fusion { osvrClientGetInterface(ctx, orientation_paths["yawAccurate"].asCString(), &(m_orientations[3])); m_alpha = orientation_paths["alpha"].asDouble(); if (orientation_paths["instantReset"].isNull()) { - m_instantReset = false; + m_do_instant_reset = false; } else { - m_instantReset = orientation_paths["instantReset"].asBool(); - if (m_instantReset) { - m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "instantReset is enabled in OSVR-Fusion."); - m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "If you notice stuttering, disable instantReset."); - } + m_do_instant_reset = true; + osvrClientGetInterface(ctx, orientation_paths["instantReset"].asCString(), &m_instant_reset_path); + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "instantReset is enabled in OSVR-Fusion."); + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "If you notice stuttering, disable instantReset."); } m_last_yaw = 0; @@ -63,7 +62,6 @@ namespace je_nourish_fusion { OSVR_ReturnCode yret = osvrGetOrientationState(m_orientations[1], timeValue, &orientation_y); OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[2], timeValue, &orientation_z); - OSVR_Vec3 rpy_x; OSVR_Vec3 rpy_y; OSVR_Vec3 rpy_z; @@ -106,11 +104,10 @@ namespace je_nourish_fusion { double a = m_alpha; double last_z = m_last_yaw; - // A factor of 2*PI is missing in angularVelocity incremental quats - at least with the HDK. - double dt = angular_v.dt * 2 * M_PI; + double dt = angular_v.dt; double z_accurate = osvrVec3GetZ(&rpy_z); - double dzdt_fast = osvrVec3GetZ(&rpy_v); + double dzdt_fast = osvrVec3GetZ(&rpy_v) * 2 * M_PI; // A factor of 2*PI is missing in angularVelocity incremental quats - at least with the HDK. double dz_fast = dt * dzdt_fast; @@ -122,15 +119,41 @@ namespace je_nourish_fusion { last_z = z_accurate; } + //OSVR_TimeValue now = osvr::util::time::getNow(); + //double t_diff = osvr::util::time::duration(now, m_last_report_time); + //m_last_report_time = now; + + OSVR_ButtonState reset_button; + OSVR_ReturnCode resret = osvrGetButtonState(m_instant_reset_path, timeValue, &reset_button); + + OSVR_TimeValue now = osvr::util::time::getNow(); + if (reset_button == OSVR_BUTTON_PRESSED) { + m_last_report_time = now; + } + double t_diff = osvr::util::time::duration(now, m_last_report_time); + + double z_displacement_limit = M_PI / 12; // Equivalent to 15 degrees + double z_angular_limit = M_PI / 360; // Less than half a degree per second double z_diff = fixUnityAngle(last_z - z_accurate); double z_out; + static int boop; + boop += 1; + if (boop >= 250) + { + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "t_diff"); + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(t_diff).c_str()); + //m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "angular_limit"); + //m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(z_angular_limit).c_str()); + boop = 0; + } + // If the yaw difference is large enough > 30 deg but the rotation rate is small enough (< 2 deg/sec) // then reset filter output to the accurate yaw value. // This fixes the issue of "smooth" external yaw resets, making them instantaneous. // Cutoff values determined empirically; in the future, perhaps these values could be configurable. - if (m_instantReset && - (z_diff < -M_PI / 6 || z_diff > M_PI / 6) && (dzdt_fast < M_PI / 90 && dzdt_fast > -M_PI / 90)) { + if (m_do_instant_reset && (t_diff < 1.0) && + ((z_diff < -z_displacement_limit) || (z_diff > z_displacement_limit)) && ((dzdt_fast < z_angular_limit) && (dzdt_fast > -z_angular_limit))) { z_out = z_accurate; } // If the difference is smaller, implement the complementary filter. diff --git a/OrientationReader.h b/OrientationReader.h index a73ea80..df8a0ff 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -35,9 +35,11 @@ namespace je_nourish_fusion { protected: OSVR_ClientInterface m_orientations[4]; osvr::clientkit::ClientContext m_ctx; - bool m_instantReset; + bool m_do_instant_reset; + OSVR_ClientInterface m_instant_reset_path; double m_alpha; double m_last_yaw; + OSVR_TimeValue m_last_report_time; }; } \ No newline at end of file From 2593689d38032ed76cd1dc1641d571277069950c Mon Sep 17 00:00:00 2001 From: Nanospork Date: Tue, 15 Aug 2017 14:12:38 -0700 Subject: [PATCH 12/19] Code cleanup --- OrientationReader.cpp | 80 ++++++++++++++++++++----------------------- OrientationReader.h | 2 +- 2 files changed, 38 insertions(+), 44 deletions(-) diff --git a/OrientationReader.cpp b/OrientationReader.cpp index c619e2d..c9b961f 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -39,16 +39,16 @@ namespace je_nourish_fusion { osvrClientGetInterface(ctx, orientation_paths["yawFast"].asCString(), &(m_orientations[2])); osvrClientGetInterface(ctx, orientation_paths["yawAccurate"].asCString(), &(m_orientations[3])); m_alpha = orientation_paths["alpha"].asDouble(); - if (orientation_paths["instantReset"].isNull()) { + if (orientation_paths["recenterButton"].isNull()) { m_do_instant_reset = false; } else { m_do_instant_reset = true; - osvrClientGetInterface(ctx, orientation_paths["instantReset"].asCString(), &m_instant_reset_path); - m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "instantReset is enabled in OSVR-Fusion."); - m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "If you notice stuttering, disable instantReset."); + osvrClientGetInterface(ctx, orientation_paths["recenterButton"].asCString(), &m_instant_reset_path); + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Recenter button detection is enabled in OSVR-Fusion."); } m_last_yaw = 0; + m_last_button_press = osvr::util::time::getNow(); m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Initialized a complementary fusion filter."); } @@ -101,77 +101,71 @@ namespace je_nourish_fusion { rpyFromQuaternion(&orientation_z, &rpy_z); rpyFromQuaternion(&angular_v.incrementalRotation, &rpy_v); - double a = m_alpha; - double last_z = m_last_yaw; + double a = m_alpha; // Grab filter threshold variable + double last_z = m_last_yaw; // Grab last yaw + double dt = angular_v.dt; // Grab timestep for use with angular velocity. Strangely, not the same as time between timeValue args - double dt = angular_v.dt; + double z_accurate = osvrVec3GetZ(&rpy_z); // Grab accurate yaw value + double dzdt_fast = osvrVec3GetZ(&rpy_v) * 2 * M_PI; // Grab fast yaw rate. A factor of 2*PI is missing in angularVelocity incremental quats - at least with the HDK. - double z_accurate = osvrVec3GetZ(&rpy_z); - double dzdt_fast = osvrVec3GetZ(&rpy_v) * 2 * M_PI; // A factor of 2*PI is missing in angularVelocity incremental quats - at least with the HDK. - - double dz_fast = dt * dzdt_fast; + double dz_fast = dt * dzdt_fast; // Create fast yaw incremental value by multiplying by timestep + // Clean up input angles dz_fast = fixUnityAngle(dz_fast); z_accurate = fixUnityAngle(z_accurate); - // Correct for the singularity at +-180 degrees + // Reset filter history if we cross the singularity at +/-180 degrees. + // This is a workaround for the filter spinning back the wrong way, but is not necessarily the best solution. if ((z_accurate < -M_PI / 2 && last_z > M_PI / 2) || (z_accurate > M_PI / 2 && last_z < -M_PI / 2)) { last_z = z_accurate; } - //OSVR_TimeValue now = osvr::util::time::getNow(); - //double t_diff = osvr::util::time::duration(now, m_last_report_time); - //m_last_report_time = now; - - OSVR_ButtonState reset_button; - OSVR_ReturnCode resret = osvrGetButtonState(m_instant_reset_path, timeValue, &reset_button); - - OSVR_TimeValue now = osvr::util::time::getNow(); - if (reset_button == OSVR_BUTTON_PRESSED) { - m_last_report_time = now; - } - double t_diff = osvr::util::time::duration(now, m_last_report_time); - double z_displacement_limit = M_PI / 12; // Equivalent to 15 degrees - double z_angular_limit = M_PI / 360; // Less than half a degree per second + double z_displacement_limit = M_PI / 18; // Equivalent to 10 degrees + double z_angular_limit = M_PI / 180; // Rotation less than one degree per second double z_diff = fixUnityAngle(last_z - z_accurate); double z_out; - static int boop; - boop += 1; - if (boop >= 250) - { - m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "t_diff"); - m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(t_diff).c_str()); - //m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "angular_limit"); - //m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, std::to_string(z_angular_limit).c_str()); - boop = 0; + // Read the instantReset button + OSVR_ButtonState reset_button; + OSVR_ReturnCode resret = osvrGetButtonState(m_instant_reset_path, timeValue, &reset_button); + + // If instantReset is enabled, store the reset button press timestamp for later comparison + double button_time_diff; + if (m_do_instant_reset) { + OSVR_TimeValue now = osvr::util::time::getNow(); + if (reset_button == OSVR_BUTTON_PRESSED) { + m_last_button_press = now; + } + button_time_diff = osvr::util::time::duration(now, m_last_button_press); } - // If the yaw difference is large enough > 30 deg but the rotation rate is small enough (< 2 deg/sec) - // then reset filter output to the accurate yaw value. - // This fixes the issue of "smooth" external yaw resets, making them instantaneous. - // Cutoff values determined empirically; in the future, perhaps these values could be configurable. - if (m_do_instant_reset && (t_diff < 1.0) && + // If instantReset is enabled, then perform some checks + // Is the difference between the previous yaw value and the current one sufficiently large? + // Is the angular velocity low? + // Was the reset button recently or currently pressed? + // If all conditions are true, then we probably detected a yaw reset, so make it snappy. + if (m_do_instant_reset && (button_time_diff < 0.5) && ((z_diff < -z_displacement_limit) || (z_diff > z_displacement_limit)) && ((dzdt_fast < z_angular_limit) && (dzdt_fast > -z_angular_limit))) { z_out = z_accurate; } - // If the difference is smaller, implement the complementary filter. + // If instantReset is not enabled or if the checks are not met, carry on with regular filter implementation. else { z_out = a*(last_z + dz_fast) + (1 - a)*(z_accurate); } - // Replace bogus results with accurate yaw. Happens sometimes on startup. if (std::isnan(z_out)) { z_out = z_accurate; } - // Keep the filter output nice and clean + // Clean up the output angle just in case z_out = fixUnityAngle(z_out); + // Store new value for next filter iteration m_last_yaw = z_out; + // Report the new orientation OSVR_Vec3 rpy; osvrVec3SetX(&rpy, osvrVec3GetX(&rpy_x)); osvrVec3SetY(&rpy, osvrVec3GetY(&rpy_y)); diff --git a/OrientationReader.h b/OrientationReader.h index df8a0ff..d2526fe 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -37,9 +37,9 @@ namespace je_nourish_fusion { osvr::clientkit::ClientContext m_ctx; bool m_do_instant_reset; OSVR_ClientInterface m_instant_reset_path; + OSVR_TimeValue m_last_button_press; double m_alpha; double m_last_yaw; - OSVR_TimeValue m_last_report_time; }; } \ No newline at end of file From 584b8ae3f073842efd8debfbdbbc64096464a3a7 Mon Sep 17 00:00:00 2001 From: Nanospork Date: Wed, 16 Aug 2017 17:55:09 -0700 Subject: [PATCH 13/19] Improved angle wrapping solution to remove stutter. Fixed angle wrap logic and used more descriptive function name. --- FusionMath.cpp | 4 ++-- FusionMath.h | 2 +- OrientationReader.cpp | 32 +++++++++++++++----------------- 3 files changed, 18 insertions(+), 20 deletions(-) diff --git a/FusionMath.cpp b/FusionMath.cpp index faa58ed..7f136f0 100644 --- a/FusionMath.cpp +++ b/FusionMath.cpp @@ -26,10 +26,10 @@ namespace je_nourish_fusion { osvrQuatSetW(quaternion, cos(r / 2) * cos(p / 2) * cos(y / 2) + sin(r / 2) * sin(p / 2) * sin(y / 2)); } - double fixUnityAngle(double angle) + double fixAngleWrap(double angle) { if (angle > M_PI) { - angle = (2 * M_PI) - angle; + angle = -(2 * M_PI) + angle; } else if (angle < -M_PI) { angle = (2 * M_PI) + angle; diff --git a/FusionMath.h b/FusionMath.h index 36a6892..510f340 100644 --- a/FusionMath.h +++ b/FusionMath.h @@ -4,6 +4,6 @@ namespace je_nourish_fusion { void rpyFromQuaternion(OSVR_Quaternion* quaternion, OSVR_Vec3* rpy); void quaternionFromRPY(OSVR_Vec3* rpy, OSVR_Quaternion* quaternion); - double fixUnityAngle(double angle); + double fixAngleWrap(double angle); } \ No newline at end of file diff --git a/OrientationReader.cpp b/OrientationReader.cpp index c9b961f..e628d3c 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -41,7 +41,8 @@ namespace je_nourish_fusion { m_alpha = orientation_paths["alpha"].asDouble(); if (orientation_paths["recenterButton"].isNull()) { m_do_instant_reset = false; - } else { + } + else { m_do_instant_reset = true; osvrClientGetInterface(ctx, orientation_paths["recenterButton"].asCString(), &m_instant_reset_path); m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Recenter button detection is enabled in OSVR-Fusion."); @@ -105,25 +106,22 @@ namespace je_nourish_fusion { double last_z = m_last_yaw; // Grab last yaw double dt = angular_v.dt; // Grab timestep for use with angular velocity. Strangely, not the same as time between timeValue args - double z_accurate = osvrVec3GetZ(&rpy_z); // Grab accurate yaw value - double dzdt_fast = osvrVec3GetZ(&rpy_v) * 2 * M_PI; // Grab fast yaw rate. A factor of 2*PI is missing in angularVelocity incremental quats - at least with the HDK. - - double dz_fast = dt * dzdt_fast; // Create fast yaw incremental value by multiplying by timestep + double z_accurate = osvrVec3GetZ(&rpy_z); // Grab accurate yaw value + double dzdt_fast = osvrVec3GetZ(&rpy_v) * 2 * M_PI; // Grab fast yaw rate. A factor of 2*PI is missing in angularVelocity incremental quats - at least with the HDK. + double dz_fast = dt * dzdt_fast; // Create fast yaw incremental value by multiplying by timestep + double z_fast = last_z + dz_fast; // Create fast yaw value. // Clean up input angles - dz_fast = fixUnityAngle(dz_fast); - z_accurate = fixUnityAngle(z_accurate); - - // Reset filter history if we cross the singularity at +/-180 degrees. - // This is a workaround for the filter spinning back the wrong way, but is not necessarily the best solution. - if ((z_accurate < -M_PI / 2 && last_z > M_PI / 2) || (z_accurate > M_PI / 2 && last_z < -M_PI / 2)) { - last_z = z_accurate; - } + z_fast = fixAngleWrap(z_fast); + z_accurate = fixAngleWrap(z_accurate); + // Handle angle wrap discrepancies (prevents the filter from spinning wrong way) + if (z_accurate < -M_PI / 2 && z_fast > M_PI / 2) { z_fast -= 2 * M_PI; } + if (z_accurate > M_PI / 2 && z_fast < -M_PI / 2) { z_fast += 2 * M_PI; } double z_displacement_limit = M_PI / 18; // Equivalent to 10 degrees double z_angular_limit = M_PI / 180; // Rotation less than one degree per second - double z_diff = fixUnityAngle(last_z - z_accurate); + double z_diff = fixAngleWrap(last_z - z_accurate); double z_out; // Read the instantReset button @@ -145,13 +143,13 @@ namespace je_nourish_fusion { // Is the angular velocity low? // Was the reset button recently or currently pressed? // If all conditions are true, then we probably detected a yaw reset, so make it snappy. - if (m_do_instant_reset && (button_time_diff < 0.5) && + if (m_do_instant_reset && (button_time_diff < 0.5) && ((z_diff < -z_displacement_limit) || (z_diff > z_displacement_limit)) && ((dzdt_fast < z_angular_limit) && (dzdt_fast > -z_angular_limit))) { z_out = z_accurate; } // If instantReset is not enabled or if the checks are not met, carry on with regular filter implementation. else { - z_out = a*(last_z + dz_fast) + (1 - a)*(z_accurate); + z_out = a*(z_fast)+(1 - a)*(z_accurate); } // Replace bogus results with accurate yaw. Happens sometimes on startup. @@ -160,7 +158,7 @@ namespace je_nourish_fusion { } // Clean up the output angle just in case - z_out = fixUnityAngle(z_out); + z_out = fixAngleWrap(z_out); // Store new value for next filter iteration m_last_yaw = z_out; From 0694057eef87403bc3b605a326b088588c31d016 Mon Sep 17 00:00:00 2001 From: Nanospork Date: Thu, 17 Aug 2017 18:21:22 -0700 Subject: [PATCH 14/19] More descriptive names. --- OrientationReader.cpp | 6 +++--- OrientationReader.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/OrientationReader.cpp b/OrientationReader.cpp index e628d3c..d10ca38 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -49,7 +49,7 @@ namespace je_nourish_fusion { } m_last_yaw = 0; - m_last_button_press = osvr::util::time::getNow(); + m_reset_press_time = osvr::util::time::getNow(); m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Initialized a complementary fusion filter."); } @@ -133,9 +133,9 @@ namespace je_nourish_fusion { if (m_do_instant_reset) { OSVR_TimeValue now = osvr::util::time::getNow(); if (reset_button == OSVR_BUTTON_PRESSED) { - m_last_button_press = now; + m_reset_press_time = now; } - button_time_diff = osvr::util::time::duration(now, m_last_button_press); + button_time_diff = osvr::util::time::duration(now, m_reset_press_time); } // If instantReset is enabled, then perform some checks diff --git a/OrientationReader.h b/OrientationReader.h index d2526fe..feb1e20 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -37,7 +37,7 @@ namespace je_nourish_fusion { osvr::clientkit::ClientContext m_ctx; bool m_do_instant_reset; OSVR_ClientInterface m_instant_reset_path; - OSVR_TimeValue m_last_button_press; + OSVR_TimeValue m_reset_press_time; double m_alpha; double m_last_yaw; }; From 62074764742e2b2e0493a05d9aa553bc05b7eeed Mon Sep 17 00:00:00 2001 From: Nanospork Date: Thu, 17 Aug 2017 20:21:06 -0700 Subject: [PATCH 15/19] First implementation of 180 degree flip. --- je_nourish_fusion.cpp | 65 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) diff --git a/je_nourish_fusion.cpp b/je_nourish_fusion.cpp index b859206..8d93ec5 100644 --- a/je_nourish_fusion.cpp +++ b/je_nourish_fusion.cpp @@ -45,6 +45,15 @@ namespace je_nourish_fusion { std::cout << "Fusion Device: Orientation Reader not created" << std::endl; } + m_useFlip = config.isMember("flipButton") && config.isMember("flipOrigin"); + if (m_useFlip) { + osvrClientGetInterface(m_ctx, config["flipButton"].asCString(), &m_flipButton); + osvrClientGetInterface(m_ctx, config["flipOrigin"].asCString(), &m_flipOriginDevice); + m_flipLastButtonValue = false; + m_isFlipped = false; + m_flipTime = osvr::util::time::getNow(); + } + m_dev->sendJsonDescriptor(je_nourish_fusion_json); m_dev->registerUpdateCallback(this); } @@ -65,6 +74,55 @@ namespace je_nourish_fusion { translation += rotation._transformVector(osvr::util::vecMap(m_offset)); } + if (m_useFlip) { + // Check for button press + OSVR_TimeValue now = osvr::util::time::getNow(); + OSVR_ButtonState flip_button_state; + OSVR_ReturnCode flipret = osvrGetButtonState(m_flipButton, &now, &flip_button_state); + double button_time_diff; + if (flip_button_state == OSVR_BUTTON_PRESSED) { + button_time_diff = osvr::util::time::duration(now, m_flipTime); + if (button_time_diff < 0.5) { + if (m_flipLastButtonValue == false) { + m_isFlipped = !m_isFlipped; + } + } + m_flipLastButtonValue = true; + m_flipTime = now; + } + else { + m_flipLastButtonValue = false; + } + + // Handle flip + if (m_isFlipped) { + OSVR_PositionState originPosition; + OSVR_ReturnCode originret = osvrGetPositionState(m_flipOriginDevice, &now, &originPosition); + + Eigen::Map originTranslation = osvr::util::vecMap(originPosition); + Eigen::Map deviceTranslation = osvr::util::vecMap(m_state.translation); + + Eigen::Vector3d flippedTranslation(2 * originTranslation.x() - deviceTranslation.x(), + deviceTranslation.y(), + 2 * originTranslation.z() - deviceTranslation.z()); + + deviceTranslation = flippedTranslation; + + OSVR_Quaternion rotateQ; + osvrQuatSetW(&rotateQ, 0); + osvrQuatSetX(&rotateQ, 0); + osvrQuatSetY(&rotateQ, 1); + osvrQuatSetZ(&rotateQ, 0); + + Eigen::Quaterniond rotateQ_eigen = osvr::util::fromQuat(rotateQ); + Eigen::Quaterniond deviceQ_eigen = osvr::util::fromQuat(m_state.rotation); + + Eigen::Quaterniond hmdRotation = rotateQ_eigen * deviceQ_eigen; + + osvr::util::toQuat(hmdRotation, m_state.rotation); + } + } + if (m_useTimestamp) { OSVR_TimeValue timeValue = m_usePositionTimestamp ? timeValuePosition : timeValueOrientation; osvrDeviceTrackerSendPoseTimestamped(*m_dev, m_tracker, &m_state, 0, &timeValue); @@ -93,6 +151,13 @@ namespace je_nourish_fusion { bool m_useTimestamp; bool m_usePositionTimestamp; + + OSVR_ClientInterface m_flipButton; + OSVR_ClientInterface m_flipOriginDevice; + bool m_useFlip; + bool m_flipLastButtonValue; + bool m_isFlipped = false; + OSVR_TimeValue m_flipTime; }; class FusionDeviceConstructor { From 80d5a26df18a984d3faa6abbeb93a3271569ce0f Mon Sep 17 00:00:00 2001 From: Nanospork Date: Thu, 17 Aug 2017 20:53:50 -0700 Subject: [PATCH 16/19] Fixed flip position offset. --- je_nourish_fusion.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/je_nourish_fusion.cpp b/je_nourish_fusion.cpp index 8d93ec5..edd0f55 100644 --- a/je_nourish_fusion.cpp +++ b/je_nourish_fusion.cpp @@ -85,6 +85,11 @@ namespace je_nourish_fusion { if (button_time_diff < 0.5) { if (m_flipLastButtonValue == false) { m_isFlipped = !m_isFlipped; + if (m_isFlipped) { + OSVR_PositionState originPosition; + OSVR_ReturnCode originret = osvrGetPositionState(m_flipOriginDevice, &now, &originPosition); + m_flipOrigin = originPosition; + } } } m_flipLastButtonValue = true; @@ -96,10 +101,10 @@ namespace je_nourish_fusion { // Handle flip if (m_isFlipped) { - OSVR_PositionState originPosition; - OSVR_ReturnCode originret = osvrGetPositionState(m_flipOriginDevice, &now, &originPosition); + //OSVR_PositionState originPosition; + //OSVR_ReturnCode originret = osvrGetPositionState(m_flipOriginDevice, &now, &originPosition); - Eigen::Map originTranslation = osvr::util::vecMap(originPosition); + Eigen::Map originTranslation = osvr::util::vecMap(m_flipOrigin); Eigen::Map deviceTranslation = osvr::util::vecMap(m_state.translation); Eigen::Vector3d flippedTranslation(2 * originTranslation.x() - deviceTranslation.x(), @@ -157,6 +162,7 @@ namespace je_nourish_fusion { bool m_useFlip; bool m_flipLastButtonValue; bool m_isFlipped = false; + OSVR_Vec3 m_flipOrigin; OSVR_TimeValue m_flipTime; }; From 28336a4e2206dace111ea2df888e4c65a0976ffe Mon Sep 17 00:00:00 2001 From: Michael Speth Date: Mon, 21 Aug 2017 17:00:30 +1200 Subject: [PATCH 17/19] Fix cmake for linux compilation. --- .gitignore | 1 + CMakeLists.txt | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build diff --git a/CMakeLists.txt b/CMakeLists.txt index b5f0a71..fc8b493 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.12) project(FusionPlugin) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set( CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH} ) find_package(osvr REQUIRED) @@ -26,4 +28,4 @@ osvr_add_plugin(NAME je_nourish_fusion FusionMath.cpp "${CMAKE_CURRENT_BINARY_DIR}/je_nourish_fusion_json.h") -target_link_libraries(je_nourish_fusion osvr::osvrClientKitCpp osvr::osvrAnalysisPluginKit jsoncpp_lib) \ No newline at end of file +target_link_libraries(je_nourish_fusion osvr::osvrClientKitCpp osvr::osvrAnalysisPluginKit jsoncpp_lib) From ad1b59bda42e78f7f2e9a6e2365de82d7047c4cb Mon Sep 17 00:00:00 2001 From: Nanospork Date: Tue, 26 Sep 2017 23:04:30 -0700 Subject: [PATCH 18/19] Some cleanup --- je_nourish_fusion.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/je_nourish_fusion.cpp b/je_nourish_fusion.cpp index edd0f55..e0a2ac5 100644 --- a/je_nourish_fusion.cpp +++ b/je_nourish_fusion.cpp @@ -101,9 +101,6 @@ namespace je_nourish_fusion { // Handle flip if (m_isFlipped) { - //OSVR_PositionState originPosition; - //OSVR_ReturnCode originret = osvrGetPositionState(m_flipOriginDevice, &now, &originPosition); - Eigen::Map originTranslation = osvr::util::vecMap(m_flipOrigin); Eigen::Map deviceTranslation = osvr::util::vecMap(m_state.translation); @@ -114,12 +111,12 @@ namespace je_nourish_fusion { deviceTranslation = flippedTranslation; OSVR_Quaternion rotateQ; - osvrQuatSetW(&rotateQ, 0); - osvrQuatSetX(&rotateQ, 0); - osvrQuatSetY(&rotateQ, 1); + osvrQuatSetW(&rotateQ, 0); + osvrQuatSetX(&rotateQ, 0); + osvrQuatSetY(&rotateQ, 1); osvrQuatSetZ(&rotateQ, 0); - Eigen::Quaterniond rotateQ_eigen = osvr::util::fromQuat(rotateQ); + Eigen::Quaterniond rotateQ_eigen = osvr::util::fromQuat(rotateQ); Eigen::Quaterniond deviceQ_eigen = osvr::util::fromQuat(m_state.rotation); Eigen::Quaterniond hmdRotation = rotateQ_eigen * deviceQ_eigen; From c44b36e6d97ac6836d74107b12446d79b6b973ef Mon Sep 17 00:00:00 2001 From: Nanospork Date: Sat, 2 Dec 2017 12:21:02 -0800 Subject: [PATCH 19/19] Got manual 'soft' yaw reset (ignores translational coordinate system) working in FilteredOrientationReader. Still needs cleanup. --- OrientationReader.cpp | 23 ++++++++++++++++++++--- OrientationReader.h | 6 +++++- 2 files changed, 25 insertions(+), 4 deletions(-) diff --git a/OrientationReader.cpp b/OrientationReader.cpp index d10ca38..5c1e246 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -39,6 +39,7 @@ namespace je_nourish_fusion { osvrClientGetInterface(ctx, orientation_paths["yawFast"].asCString(), &(m_orientations[2])); osvrClientGetInterface(ctx, orientation_paths["yawAccurate"].asCString(), &(m_orientations[3])); m_alpha = orientation_paths["alpha"].asDouble(); + m_do_soft_reset = orientation_paths["softReset"].asBool(); if (orientation_paths["recenterButton"].isNull()) { m_do_instant_reset = false; } @@ -46,6 +47,10 @@ namespace je_nourish_fusion { m_do_instant_reset = true; osvrClientGetInterface(ctx, orientation_paths["recenterButton"].asCString(), &m_instant_reset_path); m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Recenter button detection is enabled in OSVR-Fusion."); + if (m_do_soft_reset) { + m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Soft recenter is enabled in OSVR-Fusion."); + osvrRegisterButtonCallback(m_instant_reset_path, &(je_nourish_fusion::soft_reset_callback), this); + } } m_last_yaw = 0; @@ -54,6 +59,18 @@ namespace je_nourish_fusion { m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Initialized a complementary fusion filter."); } + void soft_reset_callback(void* userdata, const OSVR_TimeValue *timestamp, const OSVR_ButtonReport *report) { + FilteredOrientationReader *reader = (FilteredOrientationReader*)userdata; + if (report->state == OSVR_BUTTON_PRESSED) { + reader->m_yaw_offset = reader->m_yaw_raw; + reader->m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Button was pressed."); + } + reader->m_ctx.log(OSVR_LogLevel::OSVR_LOGLEVEL_INFO, "Button callback received."); + //std::cout << "Got button report: button is " << (report->state ? "pressed" : "released") << std::endl << std::flush; + + } + + OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) { OSVR_OrientationState orientation_x; OSVR_OrientationState orientation_y; @@ -143,13 +160,14 @@ namespace je_nourish_fusion { // Is the angular velocity low? // Was the reset button recently or currently pressed? // If all conditions are true, then we probably detected a yaw reset, so make it snappy. - if (m_do_instant_reset && (button_time_diff < 0.5) && + if (m_do_instant_reset && !m_do_soft_reset && (button_time_diff < 0.5) && ((z_diff < -z_displacement_limit) || (z_diff > z_displacement_limit)) && ((dzdt_fast < z_angular_limit) && (dzdt_fast > -z_angular_limit))) { z_out = z_accurate; } // If instantReset is not enabled or if the checks are not met, carry on with regular filter implementation. else { z_out = a*(z_fast)+(1 - a)*(z_accurate); + m_yaw_raw = z_accurate; } // Replace bogus results with accurate yaw. Happens sometimes on startup. @@ -167,11 +185,10 @@ namespace je_nourish_fusion { OSVR_Vec3 rpy; osvrVec3SetX(&rpy, osvrVec3GetX(&rpy_x)); osvrVec3SetY(&rpy, osvrVec3GetY(&rpy_y)); - osvrVec3SetZ(&rpy, z_out); + osvrVec3SetZ(&rpy, z_out - m_yaw_offset); quaternionFromRPY(&rpy, orientation); return OSVR_RETURN_SUCCESS; } - } \ No newline at end of file diff --git a/OrientationReader.h b/OrientationReader.h index feb1e20..2bd1088 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -32,14 +32,18 @@ namespace je_nourish_fusion { public: FilteredOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths); OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue); + double m_yaw_raw; + double m_yaw_offset; + osvr::clientkit::ClientContext m_ctx; protected: OSVR_ClientInterface m_orientations[4]; - osvr::clientkit::ClientContext m_ctx; bool m_do_instant_reset; + bool m_do_soft_reset; OSVR_ClientInterface m_instant_reset_path; OSVR_TimeValue m_reset_press_time; double m_alpha; double m_last_yaw; }; + void soft_reset_callback(void* userdata, const OSVR_TimeValue *timestamp, const OSVR_ButtonReport *report); } \ No newline at end of file