Implements a Complementary Filter for intelligently fusing yaw from multiple sources

.gitignore

@@ -0,0 +1 @@
+build

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)
+target_link_libraries(je_nourish_fusion osvr::osvrClientKitCpp osvr::osvrAnalysisPluginKit jsoncpp_lib)

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 fixAngleWrap(double angle)
+	{
+		if (angle > M_PI) {
+			angle = -(2 * M_PI) + angle;
+		}
+		else if (angle < -M_PI) {
+			angle = (2 * M_PI) + angle;
+		}
+		return angle;
+	}
+
 }

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 fixAngleWrap(double angle);
 
 }

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,44 @@ namespace je_nourish_fusion {
 		osvrClientGetInterface(ctx, orientation_paths["yaw"].asCString(), &(m_orientations[2]));
 	}
 
+	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]));
+		m_alpha = orientation_paths["alpha"].asDouble();
+		m_do_soft_reset = orientation_paths["softReset"].asBool();
+		if (orientation_paths["recenterButton"].isNull()) {
+			m_do_instant_reset = false;
+		}
+		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.");
+			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;
+		m_reset_press_time = osvr::util::time::getNow();
+
+		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;
@@ -56,4 +98,97 @@ 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_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[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_v;
+
+		rpyFromQuaternion(&orientation_x, &rpy_x);
+		rpyFromQuaternion(&orientation_y, &rpy_y);
+		rpyFromQuaternion(&orientation_z, &rpy_z);
+		rpyFromQuaternion(&angular_v.incrementalRotation, &rpy_v);
+
+		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 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
+		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 = fixAngleWrap(last_z - z_accurate);
+		double z_out;
+
+		// 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_reset_press_time = now;
+			}
+			button_time_diff = osvr::util::time::duration(now, m_reset_press_time);
+		}
+
+		// 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 && !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.
+		if (std::isnan(z_out)) {
+			z_out = z_accurate;
+		}
+
+		// Clean up the output angle just in case
+		z_out = fixAngleWrap(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));
+		osvrVec3SetZ(&rpy, z_out - m_yaw_offset);
+
+		quaternionFromRPY(&rpy, orientation);
+
+		return OSVR_RETURN_SUCCESS;
+	}
 }

OrientationReader.h

@@ -28,4 +28,22 @@ 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);
+		double m_yaw_raw;
+		double m_yaw_offset;
+		osvr::clientkit::ClientContext m_ctx;
+	protected:
+		OSVR_ClientInterface m_orientations[4];
+		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);
 }

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 home-made gyro device
+    		{
+    			"plugin": "je_nourish_fusion",
+    			"driver": "FusionDevice",
+    			"params": {
+    				"name": "Gyro_Kinect_Left",
+    				"position": "/je_nourish_kinect/KinectV2/semantic/body1/arms/left/hand",
+    				"orientation": {
+    					"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, favoring speed.
+						"alpha": 0.99
+    				},
+					// Pass the faster timestamp from the gyro device 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/Gyro_Kinect_Left/tracker/0"
     	}
-    }
+    }

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,57 @@ 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;
+							if (m_isFlipped) {
+								OSVR_PositionState originPosition;
+								OSVR_ReturnCode originret = osvrGetPositionState(m_flipOriginDevice, &now, &originPosition);
+								m_flipOrigin = originPosition;
+							}
+						}
+					}
+					m_flipLastButtonValue = true;
+					m_flipTime = now;
+				}
+				else {
+					m_flipLastButtonValue = false;
+				}
+
+				// Handle flip
+				if (m_isFlipped) {
+					Eigen::Map<Eigen::Vector3d> originTranslation = osvr::util::vecMap(m_flipOrigin);
+					Eigen::Map<Eigen::Vector3d> 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 +153,14 @@ 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_Vec3 m_flipOrigin;
+		OSVR_TimeValue m_flipTime;
 	};
 
 	class FusionDeviceConstructor {