├── CMakeLists.txt ├── FusionMath.cpp ├── FusionMath.h ├── LICENSE ├── OrientationReader.cpp ├── OrientationReader.h ├── PositionReader.cpp ├── PositionReader.h ├── README.md ├── cmake └── FindEigen3.cmake ├── je_nourish_fusion.cpp ├── je_nourish_fusion.json └── stdafx.h /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(FusionPlugin) 3 | 4 | set( CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH} ) 5 | 6 | find_package(osvr REQUIRED) 7 | find_package(jsoncpp REQUIRED) 8 | find_package(Eigen3 REQUIRED) 9 | 10 | include_directories("${EIGEN3_INCLUDE_DIR}" "${CMAKE_CURRENT_BINARY_DIR}") 11 | 12 | osvr_convert_json(je_nourish_fusion_json 13 | je_nourish_fusion.json 14 | "${CMAKE_CURRENT_BINARY_DIR}/je_nourish_fusion_json.h") 15 | 16 | osvr_add_plugin(NAME je_nourish_fusion 17 | CPP 18 | SOURCES 19 | je_nourish_fusion.cpp 20 | PositionReader.h 21 | PositionReader.cpp 22 | OrientationReader.h 23 | OrientationReader.cpp 24 | stdafx.h 25 | FusionMath.h 26 | FusionMath.cpp 27 | "${CMAKE_CURRENT_BINARY_DIR}/je_nourish_fusion_json.h") 28 | 29 | target_link_libraries(je_nourish_fusion osvr::osvrClientKitCpp osvr::osvrAnalysisPluginKit jsoncpp_lib) -------------------------------------------------------------------------------- /FusionMath.cpp: -------------------------------------------------------------------------------- 1 | #include "stdafx.h" 2 | 3 | namespace je_nourish_fusion { 4 | 5 | void rpyFromQuaternion(OSVR_Quaternion* quaternion, OSVR_Vec3* rpy) { 6 | double q[4] = { 7 | osvrQuatGetW(quaternion), 8 | osvrQuatGetZ(quaternion), 9 | osvrQuatGetX(quaternion), 10 | osvrQuatGetY(quaternion) 11 | }; 12 | 13 | osvrVec3SetX(rpy, -atan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]))); 14 | osvrVec3SetY(rpy, -asin(2 * (q[0] * q[2] - q[3] * q[1]))); 15 | osvrVec3SetZ(rpy, -atan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3]))); 16 | } 17 | 18 | void quaternionFromRPY(OSVR_Vec3* rpy, OSVR_Quaternion* quaternion) { 19 | double r = -osvrVec3GetX(rpy); 20 | double p = -osvrVec3GetY(rpy); 21 | double y = -osvrVec3GetZ(rpy); 22 | 23 | osvrQuatSetZ(quaternion, sin(r / 2) * cos(p / 2) * cos(y / 2) - cos(r / 2) * sin(p / 2) * sin(y / 2)); 24 | osvrQuatSetX(quaternion, cos(r / 2) * sin(p / 2) * cos(y / 2) + sin(r / 2) * cos(p / 2) * sin(y / 2)); 25 | osvrQuatSetY(quaternion, cos(r / 2) * cos(p / 2) * sin(y / 2) - sin(r / 2) * sin(p / 2) * cos(y / 2)); 26 | osvrQuatSetW(quaternion, cos(r / 2) * cos(p / 2) * cos(y / 2) + sin(r / 2) * sin(p / 2) * sin(y / 2)); 27 | } 28 | 29 | } -------------------------------------------------------------------------------- /FusionMath.h: -------------------------------------------------------------------------------- 1 | #include "stdafx.h" 2 | 3 | namespace je_nourish_fusion { 4 | 5 | void rpyFromQuaternion(OSVR_Quaternion* quaternion, OSVR_Vec3* rpy); 6 | void quaternionFromRPY(OSVR_Vec3* rpy, OSVR_Quaternion* quaternion); 7 | 8 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | Copyright (c) 2016 Steve Le Roy Harris 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated 5 | documentation files (the "Software"), to deal in the Software without restriction, including without limitation 6 | the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, 7 | and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 8 | 9 | The above copyright notice and this permission notice shall be included in all copies or substantial portions 10 | of the Software. 11 | 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 13 | TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 14 | THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF 15 | CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 16 | DEALINGS IN THE SOFTWARE. 17 | -------------------------------------------------------------------------------- /OrientationReader.cpp: -------------------------------------------------------------------------------- 1 | #include "stdafx.h" 2 | #include 3 | 4 | namespace je_nourish_fusion { 5 | 6 | IOrientationReader* OrientationReaderFactory::getReader(OSVR_ClientContext ctx, Json::Value config) { 7 | IOrientationReader* reader = NULL; 8 | 9 | if (config.isString()) { 10 | reader = new SingleOrientationReader(ctx, config.asString()); 11 | } 12 | if (config.isObject() && config.isMember("roll") && config.isMember("pitch") && config.isMember("yaw")) { 13 | reader = new CombinedOrientationReader(ctx, config); 14 | } 15 | 16 | return reader; 17 | } 18 | 19 | SingleOrientationReader::SingleOrientationReader(OSVR_ClientContext ctx, std::string orientation_path) { 20 | osvrClientGetInterface(ctx, orientation_path.c_str(), &m_orientation); 21 | } 22 | OSVR_ReturnCode SingleOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) { 23 | return osvrGetOrientationState(m_orientation, timeValue, orientation); 24 | } 25 | 26 | CombinedOrientationReader::CombinedOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths) { 27 | osvrClientGetInterface(ctx, orientation_paths["roll"].asCString(), &(m_orientations[0])); 28 | osvrClientGetInterface(ctx, orientation_paths["pitch"].asCString(), &(m_orientations[1])); 29 | osvrClientGetInterface(ctx, orientation_paths["yaw"].asCString(), &(m_orientations[2])); 30 | } 31 | 32 | OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) { 33 | OSVR_OrientationState orientation_x; 34 | OSVR_OrientationState orientation_y; 35 | OSVR_OrientationState orientation_z; 36 | 37 | OSVR_ReturnCode xret = osvrGetOrientationState(m_orientations[0], timeValue, &orientation_x); 38 | OSVR_ReturnCode yret = osvrGetOrientationState(m_orientations[1], timeValue, &orientation_y); 39 | OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[2], timeValue, &orientation_z); 40 | 41 | OSVR_Vec3 rpy_x; 42 | OSVR_Vec3 rpy_y; 43 | OSVR_Vec3 rpy_z; 44 | 45 | rpyFromQuaternion(&orientation_x, &rpy_x); 46 | rpyFromQuaternion(&orientation_y, &rpy_y); 47 | rpyFromQuaternion(&orientation_z, &rpy_z); 48 | 49 | OSVR_Vec3 rpy; 50 | osvrVec3SetX(&rpy, osvrVec3GetX(&rpy_x)); 51 | osvrVec3SetY(&rpy, osvrVec3GetY(&rpy_y)); 52 | osvrVec3SetZ(&rpy, osvrVec3GetZ(&rpy_z)); 53 | 54 | quaternionFromRPY(&rpy, orientation); 55 | 56 | return OSVR_RETURN_SUCCESS; 57 | } 58 | 59 | } -------------------------------------------------------------------------------- /OrientationReader.h: -------------------------------------------------------------------------------- 1 | #include "stdafx.h" 2 | 3 | namespace je_nourish_fusion { 4 | 5 | class IOrientationReader { 6 | public: 7 | virtual OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) = 0; 8 | }; 9 | 10 | class OrientationReaderFactory { 11 | public: 12 | static IOrientationReader* getReader(OSVR_ClientContext ctx, Json::Value config); 13 | }; 14 | 15 | class SingleOrientationReader : public IOrientationReader { 16 | public: 17 | SingleOrientationReader(OSVR_ClientContext ctx, std::string orientation_path); 18 | OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue); 19 | protected: 20 | OSVR_ClientInterface m_orientation; 21 | }; 22 | 23 | class CombinedOrientationReader : public IOrientationReader { 24 | public: 25 | CombinedOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths); 26 | OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue); 27 | protected: 28 | OSVR_ClientInterface m_orientations[3]; 29 | }; 30 | 31 | } -------------------------------------------------------------------------------- /PositionReader.cpp: -------------------------------------------------------------------------------- 1 | #include "stdafx.h" 2 | 3 | namespace je_nourish_fusion { 4 | 5 | IPositionReader* PositionReaderFactory::getReader(OSVR_ClientContext ctx, Json::Value config) { 6 | IPositionReader* reader = NULL; 7 | 8 | if (config.isString()) { 9 | reader = new SinglePositionReader(ctx, config.asString()); 10 | } 11 | if (config.isObject() && config.isMember("x") && config.isMember("y") && config.isMember("z")) { 12 | reader = new CombinedPositionReader(ctx, config); 13 | } 14 | 15 | return reader; 16 | } 17 | 18 | SinglePositionReader::SinglePositionReader(OSVR_ClientContext ctx, std::string position_path) { 19 | osvrClientGetInterface(ctx, position_path.c_str(), &m_position); 20 | } 21 | OSVR_ReturnCode SinglePositionReader::update(OSVR_PositionState* position, OSVR_TimeValue* timeValue) { 22 | return osvrGetPositionState(m_position, timeValue, position); 23 | } 24 | 25 | CombinedPositionReader::CombinedPositionReader(OSVR_ClientContext ctx, Json::Value position_paths) { 26 | osvrClientGetInterface(ctx, position_paths["x"].asCString(), &(m_positions[0])); 27 | osvrClientGetInterface(ctx, position_paths["y"].asCString(), &(m_positions[1])); 28 | osvrClientGetInterface(ctx, position_paths["z"].asCString(), &(m_positions[2])); 29 | } 30 | OSVR_ReturnCode CombinedPositionReader::update(OSVR_PositionState* position, OSVR_TimeValue* timeValue) { 31 | OSVR_PositionState position_x; 32 | OSVR_PositionState position_y; 33 | OSVR_PositionState position_z; 34 | 35 | OSVR_ReturnCode xret = osvrGetPositionState(m_positions[0], timeValue, &position_x); 36 | OSVR_ReturnCode yret = osvrGetPositionState(m_positions[1], timeValue, &position_y); 37 | OSVR_ReturnCode zret = osvrGetPositionState(m_positions[2], timeValue, &position_z); 38 | 39 | if (xret == OSVR_RETURN_SUCCESS) { 40 | osvrVec3SetX(position, osvrVec3GetX(&position_x)); 41 | } 42 | if (yret == OSVR_RETURN_SUCCESS) { 43 | osvrVec3SetY(position, osvrVec3GetY(&position_y)); 44 | } 45 | if (zret == OSVR_RETURN_SUCCESS) { 46 | osvrVec3SetZ(position, osvrVec3GetZ(&position_z)); 47 | } 48 | 49 | return OSVR_RETURN_SUCCESS; 50 | } 51 | 52 | } -------------------------------------------------------------------------------- /PositionReader.h: -------------------------------------------------------------------------------- 1 | #include "stdafx.h" 2 | 3 | namespace je_nourish_fusion { 4 | 5 | class IPositionReader { 6 | public: 7 | virtual OSVR_ReturnCode update(OSVR_PositionState* position, OSVR_TimeValue* timeValue) = 0; 8 | }; 9 | 10 | class PositionReaderFactory { 11 | public: 12 | static IPositionReader* getReader(OSVR_ClientContext ctx, Json::Value config); 13 | }; 14 | 15 | class SinglePositionReader : public IPositionReader { 16 | public: 17 | SinglePositionReader(OSVR_ClientContext ctx, std::string position_path); 18 | OSVR_ReturnCode update(OSVR_PositionState* position, OSVR_TimeValue* timeValue); 19 | protected: 20 | OSVR_ClientInterface m_position; 21 | }; 22 | 23 | class CombinedPositionReader : public IPositionReader { 24 | public: 25 | CombinedPositionReader(OSVR_ClientContext ctx, Json::Value position_paths); 26 | OSVR_ReturnCode update(OSVR_PositionState* position, OSVR_TimeValue* timeValue); 27 | protected: 28 | OSVR_ClientInterface m_positions[3]; 29 | }; 30 | 31 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OSVR-fusion [![Donate](https://nourish.je/assets/images/donate.svg)](http://ko-fi.com/A250KJT) 2 | 3 | An OSVR plugin that creates trackers from different sources of data. For example, taking the orientation data from an HMD with no positional tracking, and the position data from a motion controller with no orientation data. 4 | 5 | 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. 6 | 7 | Build following the [standard OSVR plugin build instructions](http://resource.osvr.com/docs/OSVR-Core/TopicWritingDevicePlugin.html). 8 | 9 | ## Tracker alignment 10 | 11 | The orientation and position data will likely be misaligned, eg, you are facing forward and leaning forward, but your tracked position instead moves to the side. To correct this, align the orientation tracker with the position tracker's axes and run osvr_reset_yaw on the orientation tracker. 12 | 13 | For example, with the OSVR HDK and a Kinect, you would place the HDK in front of the Kinect, pointing towards it, then run 14 | 15 | osvr_reset_yaw.exe --path "/com_osvr_Multiserver/OSVRHackerDevKitPrediction0/semantic/hmd" 16 | 17 | This replaces the `alignInitialOrientation` option in previous versions. 18 | 19 | ## Maintainers 20 | 21 | [nanospork](https://github.com/nanospork) and [simlrh](https://github.com/simlrh) 22 | 23 | ## Sample osvr_server_config.json: 24 | 25 | { 26 | "drivers": [ 27 | // Combine Oculus DK1 orientation with Kinect position 28 | { 29 | "plugin": "je_nourish_fusion", 30 | "driver": "FusionDevice", 31 | "params": { 32 | "name": "DK1_Kinectv2", 33 | "position": "/je_nourish_kinect/KinectV2/semantic/body1/head", 34 | "orientation": "/je_nourish_openhmd/Oculus Rift (Devkit)/semantic/hmd", 35 | // Eyes are above and in front of the center of the head 36 | "offsetFromRotationCenter": { 37 | "x": 0, 38 | "y": 0.01, 39 | "z": -0.05 40 | }, 41 | // Pass the timestamp from the Kinect skeleton data to OSVR 42 | "timestamp": "position" 43 | } 44 | }, 45 | // Combine more accurate pitch and roll from Wii Nunchuk with yaw and position from Kinect 46 | { 47 | "plugin": "je_nourish_fusion", 48 | "driver": "FusionDevice", 49 | "params": { 50 | "name": "Wii_Kinect_Right", 51 | "position": "/je_nourish_kinect/KinectV2/semantic/body1/arms/right/hand", 52 | "orientation": { 53 | "roll": "/je_nourish_wiimote/WiimoteDevice/semantic/wiimote1/nunchuk", 54 | "pitch": "/je_nourish_wiimote/WiimoteDevice/semantic/wiimote1/nunchuk", 55 | "yaw": "/je_nourish_kinectv2/KinectV2/semantic/body1/arms/right/hand" 56 | } 57 | } 58 | } 59 | ], 60 | "aliases": { 61 | "/me/head": "/je_nourish_fusion/DK1_Kinectv2/tracker/0", 62 | "/me/hands/right": "/je_nourish_fusion/Wii_Kinect_Right/tracker/0" 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /je_nourish_fusion.cpp: -------------------------------------------------------------------------------- 1 | #include "stdafx.h" 2 | #include 3 | 4 | // Anonymous namespace to avoid symbol collision 5 | namespace je_nourish_fusion { 6 | 7 | class FusionDevice { 8 | public: 9 | FusionDevice(OSVR_PluginRegContext ctx, Json::Value config) { 10 | osvrPose3SetIdentity(&m_state); 11 | 12 | m_useTimestamp = config.isMember("timestamp"); 13 | m_usePositionTimestamp = m_useTimestamp && config["timestamp"].asString().compare("position") == 0; 14 | 15 | if ((m_useOffset = config.isMember("offsetFromRotationCenter"))) { 16 | osvrVec3Zero(&m_offset); 17 | 18 | if (config["offsetFromRotationCenter"].isMember("x")) { 19 | osvrVec3SetX(&m_offset, config["offsetFromRotationCenter"]["x"].asDouble()); 20 | } 21 | if (config["offsetFromRotationCenter"].isMember("y")) { 22 | osvrVec3SetY(&m_offset, config["offsetFromRotationCenter"]["y"].asDouble()); 23 | } 24 | if (config["offsetFromRotationCenter"].isMember("z")) { 25 | osvrVec3SetZ(&m_offset, config["offsetFromRotationCenter"]["z"].asDouble()); 26 | } 27 | } 28 | 29 | OSVR_DeviceInitOptions opts = osvrDeviceCreateInitOptions(ctx); 30 | 31 | osvrDeviceTrackerConfigure(opts, &m_tracker); 32 | 33 | OSVR_DeviceToken token; 34 | osvrAnalysisSyncInit(ctx, config["name"].asCString(), opts, &token, &m_ctx); 35 | 36 | m_dev = new osvr::pluginkit::DeviceToken(token); 37 | 38 | m_positionReader = PositionReaderFactory::getReader(m_ctx, config["position"]); 39 | m_orientationReader = OrientationReaderFactory::getReader(m_ctx, config["orientation"]); 40 | 41 | if (m_positionReader == NULL) { 42 | std::cout << "Fusion Device: Position Reader not created" << std::endl; 43 | } 44 | if (m_orientationReader == NULL) { 45 | std::cout << "Fusion Device: Orientation Reader not created" << std::endl; 46 | } 47 | 48 | m_dev->sendJsonDescriptor(je_nourish_fusion_json); 49 | m_dev->registerUpdateCallback(this); 50 | } 51 | 52 | OSVR_ReturnCode update() { 53 | osvrClientUpdate(m_ctx); 54 | 55 | OSVR_TimeValue timeValuePosition; 56 | OSVR_TimeValue timeValueOrientation; 57 | 58 | m_positionReader->update(&m_state.translation, &timeValuePosition); 59 | m_orientationReader->update(&m_state.rotation, &timeValueOrientation); 60 | 61 | if (m_useOffset) { 62 | Eigen::Quaterniond rotation = osvr::util::fromQuat(m_state.rotation); 63 | Eigen::Map translation = osvr::util::vecMap(m_state.translation); 64 | 65 | translation += rotation._transformVector(osvr::util::vecMap(m_offset)); 66 | } 67 | 68 | if (m_useTimestamp) { 69 | OSVR_TimeValue timeValue = m_usePositionTimestamp ? timeValuePosition : timeValueOrientation; 70 | osvrDeviceTrackerSendPoseTimestamped(*m_dev, m_tracker, &m_state, 0, &timeValue); 71 | } 72 | else { 73 | osvrDeviceTrackerSendPose(*m_dev, m_tracker, &m_state, 0); 74 | } 75 | 76 | return OSVR_RETURN_SUCCESS; 77 | } 78 | 79 | private: 80 | IPositionReader* m_positionReader; 81 | IOrientationReader* m_orientationReader; 82 | 83 | OSVR_ClientContext m_ctx; 84 | OSVR_ClientInterface m_position; 85 | OSVR_ClientInterface m_orientation; 86 | 87 | osvr::pluginkit::DeviceToken* m_dev; 88 | OSVR_TrackerDeviceInterface m_tracker; 89 | OSVR_PoseState m_state; 90 | 91 | bool m_useOffset; 92 | OSVR_Vec3 m_offset; 93 | 94 | bool m_useTimestamp; 95 | bool m_usePositionTimestamp; 96 | }; 97 | 98 | class FusionDeviceConstructor { 99 | public: 100 | FusionDeviceConstructor() {} 101 | OSVR_ReturnCode operator()(OSVR_PluginRegContext ctx, const char *params) { 102 | Json::Value root; 103 | if (params) { 104 | Json::Reader r; 105 | if (!r.parse(params, root)) { 106 | std::cerr << "Could not parse parameters!" << std::endl; 107 | } 108 | } 109 | 110 | if (!root.isMember("name") || !root.isMember("position") || !root.isMember("orientation")) { 111 | std::cerr << "Warning: got configuration, but no trackers specified" 112 | << std::endl; 113 | return OSVR_RETURN_FAILURE; 114 | } 115 | 116 | osvr::pluginkit::registerObjectForDeletion( 117 | ctx, new FusionDevice(ctx, root)); 118 | 119 | return OSVR_RETURN_SUCCESS; 120 | } 121 | }; 122 | } // namespace 123 | 124 | OSVR_PLUGIN(je_nourish_fusion) { 125 | 126 | osvr::pluginkit::registerDriverInstantiationCallback( 127 | ctx, "FusionDevice", new je_nourish_fusion::FusionDeviceConstructor); 128 | 129 | return OSVR_RETURN_SUCCESS; 130 | } 131 | -------------------------------------------------------------------------------- /je_nourish_fusion.json: -------------------------------------------------------------------------------- 1 | { 2 | "deviceVendor": "Steve Le Roy Harris", 3 | "deviceName": "Tracker Fusion", 4 | "author": "Steve Le Roy Harris ", 5 | "version": 1, 6 | "lastModified": "2016-06-25T21:13:07.585Z", 7 | "interfaces": { 8 | "tracker": { 9 | "position": true, 10 | "orientation": true, 11 | "count": 1 12 | } 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /stdafx.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Internal Includes 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // Generated JSON header file 14 | #include "je_nourish_fusion_json.h" 15 | 16 | // Library/third-party includes 17 | #include 18 | 19 | // Standard includes 20 | #include 21 | #include 22 | 23 | #include "FusionMath.h" 24 | #include "PositionReader.h" 25 | #include "OrientationReader.h" --------------------------------------------------------------------------------