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) diff --git a/FusionMath.cpp b/FusionMath.cpp index 3bb006e..7f136f0 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 fixAngleWrap(double angle) + { + if (angle > M_PI) { + angle = -(2 * M_PI) + angle; + } + else if (angle < -M_PI) { + angle = (2 * M_PI) + angle; + } + return angle; + } + } \ No newline at end of file diff --git a/FusionMath.h b/FusionMath.h index 27da552..510f340 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 fixAngleWrap(double angle); } \ No newline at end of file diff --git a/OrientationReader.cpp b/OrientationReader.cpp index be3eb3d..5c1e246 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,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; + } } \ No newline at end of file diff --git a/OrientationReader.h b/OrientationReader.h index 7267845..2bd1088 100644 --- a/OrientationReader.h +++ b/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); } \ No newline at end of file diff --git a/README.md b/README.md index 65fc1fc..c217ba8 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 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" } - } + } \ No newline at end of file diff --git a/je_nourish_fusion.cpp b/je_nourish_fusion.cpp index b859206..e0a2ac5 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,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 originTranslation = osvr::util::vecMap(m_flipOrigin); + 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 +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 {