From 39ad2b25a2c4652da8684425f518d4f58ad5e263 Mon Sep 17 00:00:00 2001 From: Steve Le Roy Harris Date: Tue, 27 Sep 2016 11:31:54 +0100 Subject: [PATCH] Add timestamp; remove orienation alignment --- FusionMath.cpp | 53 ------------------------------------------- FusionMath.h | 8 ------- OrientationReader.cpp | 16 +++++-------- OrientationReader.h | 6 ++--- PositionReader.cpp | 16 +++++-------- PositionReader.h | 6 ++--- README.md | 18 +++++++++++---- je_nourish_fusion.cpp | 42 +++++++++++++++------------------- 8 files changed, 50 insertions(+), 115 deletions(-) diff --git a/FusionMath.cpp b/FusionMath.cpp index d0809f6..3bb006e 100644 --- a/FusionMath.cpp +++ b/FusionMath.cpp @@ -26,57 +26,4 @@ namespace je_nourish_fusion { osvrQuatSetW(quaternion, cos(r / 2) * cos(p / 2) * cos(y / 2) + sin(r / 2) * sin(p / 2) * sin(y / 2)); } - void invertQuaternion(OSVR_Quaternion* quaternion) { - double x = osvrQuatGetX(quaternion); - double y = osvrQuatGetY(quaternion); - double z = osvrQuatGetZ(quaternion); - double w = osvrQuatGetW(quaternion); - - double magnitude = sqrt(x*x + y*y + z*z + w*w); - - osvrQuatSetX(quaternion, -osvrQuatGetX(quaternion) / magnitude); - osvrQuatSetY(quaternion, -osvrQuatGetY(quaternion) / magnitude); - osvrQuatSetY(quaternion, -osvrQuatGetZ(quaternion) / magnitude); - - osvrQuatSetW(quaternion, osvrQuatGetW(quaternion) / magnitude); - } - - void multiplyQuaternion(OSVR_Quaternion* q1, OSVR_Quaternion* q2) { - double x = osvrQuatGetX(q1) * osvrQuatGetW(q2) + osvrQuatGetY(q1) * osvrQuatGetZ(q2) - osvrQuatGetZ(q1) * osvrQuatGetY(q2) + osvrQuatGetW(q1) * osvrQuatGetX(q2); - double y = -osvrQuatGetX(q1) * osvrQuatGetZ(q2) + osvrQuatGetY(q1) * osvrQuatGetW(q2) + osvrQuatGetZ(q1) * osvrQuatGetX(q2) + osvrQuatGetW(q1) * osvrQuatGetY(q2); - double z = osvrQuatGetX(q1) * osvrQuatGetY(q2) - osvrQuatGetY(q1) * osvrQuatGetX(q2) + osvrQuatGetZ(q1) * osvrQuatGetW(q2) + osvrQuatGetW(q1) * osvrQuatGetZ(q2); - double w = -osvrQuatGetX(q1) * osvrQuatGetX(q2) - osvrQuatGetY(q1) * osvrQuatGetY(q2) - osvrQuatGetZ(q1) * osvrQuatGetZ(q2) + osvrQuatGetW(q1) * osvrQuatGetW(q2); - - osvrQuatSetX(q2, x); - osvrQuatSetY(q2, y); - osvrQuatSetZ(q2, z); - osvrQuatSetW(q2, w); - } - - void offsetTranslation(OSVR_Vec3* translation_offset, OSVR_Vec3* translation) { - osvrVec3SetX(translation, osvrVec3GetX(translation) - osvrVec3GetX(translation_offset)); - osvrVec3SetY(translation, osvrVec3GetY(translation) - osvrVec3GetY(translation_offset)); - osvrVec3SetZ(translation, osvrVec3GetZ(translation) - osvrVec3GetZ(translation_offset)); - } - - void setupAlign(OSVR_PoseState* offset, OSVR_PoseState* poseState) { - OSVR_Vec3* translation = &(poseState->translation); - osvrVec3SetX(&(offset->translation), osvrVec3GetX(translation)); - osvrVec3SetY(&(offset->translation), osvrVec3GetY(translation)); - osvrVec3SetZ(&(offset->translation), osvrVec3GetZ(translation)); - - OSVR_Quaternion* rotation = &(poseState->rotation); - osvrQuatSetX(&(offset->rotation), osvrQuatGetX(rotation)); - osvrQuatSetY(&(offset->rotation), osvrQuatGetY(rotation)); - osvrQuatSetZ(&(offset->rotation), osvrQuatGetZ(rotation)); - osvrQuatSetW(&(offset->rotation), osvrQuatGetW(rotation)); - - invertQuaternion(&(offset->rotation)); - } - - void applyAlign(OSVR_PoseState* offset, OSVR_PoseState* poseState) { - offsetTranslation(&(offset->translation), &(poseState->translation)); - multiplyQuaternion(&(offset->rotation), &(poseState->rotation)); - } - } \ No newline at end of file diff --git a/FusionMath.h b/FusionMath.h index a2963b2..27da552 100644 --- a/FusionMath.h +++ b/FusionMath.h @@ -5,12 +5,4 @@ namespace je_nourish_fusion { void rpyFromQuaternion(OSVR_Quaternion* quaternion, OSVR_Vec3* rpy); void quaternionFromRPY(OSVR_Vec3* rpy, OSVR_Quaternion* quaternion); - void invertQuaternion(OSVR_Quaternion* quaternion); - void multiplyQuaternion(OSVR_Quaternion* q1, OSVR_Quaternion* q2); - - void offsetTranslation(OSVR_Vec3* translation_offset, OSVR_Vec3* translation); - - void setupAlign(OSVR_PoseState* offset, OSVR_PoseState* poseState); - void applyAlign(OSVR_PoseState* offset, OSVR_PoseState* poseState); - } \ No newline at end of file diff --git a/OrientationReader.cpp b/OrientationReader.cpp index 065331a..be3eb3d 100644 --- a/OrientationReader.cpp +++ b/OrientationReader.cpp @@ -19,10 +19,8 @@ namespace je_nourish_fusion { SingleOrientationReader::SingleOrientationReader(OSVR_ClientContext ctx, std::string orientation_path) { osvrClientGetInterface(ctx, orientation_path.c_str(), &m_orientation); } - OSVR_ReturnCode SingleOrientationReader::update(OSVR_OrientationState* orientation) { - OSVR_TimeValue timestamp; - - return osvrGetOrientationState(m_orientation, ×tamp, orientation); + OSVR_ReturnCode SingleOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) { + return osvrGetOrientationState(m_orientation, timeValue, orientation); } CombinedOrientationReader::CombinedOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths) { @@ -31,16 +29,14 @@ namespace je_nourish_fusion { osvrClientGetInterface(ctx, orientation_paths["yaw"].asCString(), &(m_orientations[2])); } - OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation) { + OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) { OSVR_OrientationState orientation_x; OSVR_OrientationState orientation_y; OSVR_OrientationState orientation_z; - OSVR_TimeValue timestamp; - - OSVR_ReturnCode xret = osvrGetOrientationState(m_orientations[0], ×tamp, &orientation_x); - OSVR_ReturnCode yret = osvrGetOrientationState(m_orientations[1], ×tamp, &orientation_y); - OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[2], ×tamp, &orientation_z); + 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_Vec3 rpy_x; OSVR_Vec3 rpy_y; diff --git a/OrientationReader.h b/OrientationReader.h index 0742f2a..7267845 100644 --- a/OrientationReader.h +++ b/OrientationReader.h @@ -4,7 +4,7 @@ namespace je_nourish_fusion { class IOrientationReader { public: - virtual OSVR_ReturnCode update(OSVR_OrientationState* orientation) = 0; + virtual OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) = 0; }; class OrientationReaderFactory { @@ -15,7 +15,7 @@ namespace je_nourish_fusion { class SingleOrientationReader : public IOrientationReader { public: SingleOrientationReader(OSVR_ClientContext ctx, std::string orientation_path); - OSVR_ReturnCode update(OSVR_OrientationState* orientation); + OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue); protected: OSVR_ClientInterface m_orientation; }; @@ -23,7 +23,7 @@ namespace je_nourish_fusion { class CombinedOrientationReader : public IOrientationReader { public: CombinedOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths); - OSVR_ReturnCode update(OSVR_OrientationState* orientation); + OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue); protected: OSVR_ClientInterface m_orientations[3]; }; diff --git a/PositionReader.cpp b/PositionReader.cpp index c2c30ea..db5006d 100644 --- a/PositionReader.cpp +++ b/PositionReader.cpp @@ -18,10 +18,8 @@ namespace je_nourish_fusion { SinglePositionReader::SinglePositionReader(OSVR_ClientContext ctx, std::string position_path) { osvrClientGetInterface(ctx, position_path.c_str(), &m_position); } - OSVR_ReturnCode SinglePositionReader::update(OSVR_PositionState* position) { - OSVR_TimeValue timestamp; - - return osvrGetPositionState(m_position, ×tamp, position); + OSVR_ReturnCode SinglePositionReader::update(OSVR_PositionState* position, OSVR_TimeValue* timeValue) { + return osvrGetPositionState(m_position, timeValue, position); } CombinedPositionReader::CombinedPositionReader(OSVR_ClientContext ctx, Json::Value position_paths) { @@ -29,16 +27,14 @@ namespace je_nourish_fusion { osvrClientGetInterface(ctx, position_paths["y"].asCString(), &(m_positions[1])); osvrClientGetInterface(ctx, position_paths["z"].asCString(), &(m_positions[2])); } - OSVR_ReturnCode CombinedPositionReader::update(OSVR_PositionState* position) { + OSVR_ReturnCode CombinedPositionReader::update(OSVR_PositionState* position, OSVR_TimeValue* timeValue) { OSVR_PositionState position_x; OSVR_PositionState position_y; OSVR_PositionState position_z; - OSVR_TimeValue timestamp; - - OSVR_ReturnCode xret = osvrGetPositionState(m_positions[0], ×tamp, &position_x); - OSVR_ReturnCode yret = osvrGetPositionState(m_positions[1], ×tamp, &position_y); - OSVR_ReturnCode zret = osvrGetPositionState(m_positions[2], ×tamp, &position_z); + OSVR_ReturnCode xret = osvrGetPositionState(m_positions[0], timeValue, &position_x); + OSVR_ReturnCode yret = osvrGetPositionState(m_positions[1], timeValue, &position_y); + OSVR_ReturnCode zret = osvrGetPositionState(m_positions[2], timeValue, &position_z); if (xret == OSVR_RETURN_SUCCESS) { osvrVec3SetX(position, osvrVec3GetX(&position_x)); diff --git a/PositionReader.h b/PositionReader.h index 9219725..e2cd4b6 100644 --- a/PositionReader.h +++ b/PositionReader.h @@ -4,7 +4,7 @@ namespace je_nourish_fusion { class IPositionReader { public: - virtual OSVR_ReturnCode update(OSVR_PositionState* position) = 0; + virtual OSVR_ReturnCode update(OSVR_PositionState* position, OSVR_TimeValue* timeValue) = 0; }; class PositionReaderFactory { @@ -15,7 +15,7 @@ namespace je_nourish_fusion { class SinglePositionReader : public IPositionReader { public: SinglePositionReader(OSVR_ClientContext ctx, std::string position_path); - OSVR_ReturnCode update(OSVR_PositionState* position); + OSVR_ReturnCode update(OSVR_PositionState* position, OSVR_TimeValue* timeValue); protected: OSVR_ClientInterface m_position; }; @@ -23,7 +23,7 @@ namespace je_nourish_fusion { class CombinedPositionReader : public IPositionReader { public: CombinedPositionReader(OSVR_ClientContext ctx, Json::Value position_paths); - OSVR_ReturnCode update(OSVR_PositionState* position); + OSVR_ReturnCode update(OSVR_PositionState* position, OSVR_TimeValue* timeValue); protected: OSVR_ClientInterface m_positions[3]; }; diff --git a/README.md b/README.md index 4412bec..251ebb3 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,17 @@ It can also combine axes from different trackers, eg taking pitch and roll from Build following the [standard OSVR plugin build instructions](http://resource.osvr.com/docs/OSVR-Core/TopicWritingDevicePlugin.html). -Sample osvr_server_config.json: +## Tracker alignment + +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. + +For example, with the OSVR HDK and a Kinect, you would place the HDK in front of the Kinect, pointing towards it, then run + + osvr_reset_yaw.exe --path "/com_osvr_Multiserver/OSVRHackerDevKitPrediction0/semantic/hmd" + +This replaces the `alignInitialOrientation` option in previous versions. + +## Sample osvr_server_config.json: { "drivers": [ @@ -18,14 +28,14 @@ Sample osvr_server_config.json: "name": "DK1_Kinectv2", "position": "/je_nourish_kinectv2/KinectV2/semantic/body1/head", "orientation": "/je_nourish_openhmd/Oculus Rift (Devkit)/semantic/hmd", - // Align DK1 and Kinect axes (point hmd directly at kinect on startup) - "alignInitialOrientation": true, // Eyes are above and in front of the center of the head "offsetFromRotationCenter": { "x": 0, "y": 0.01, "z": -0.05 - } + }, + // Pass the timestamp from the Kinect skeleton data to OSVR + "timestamp": "position" } }, // Combine more accurate pitch and roll from Wii Nunchuk with yaw and position from Kinect diff --git a/je_nourish_fusion.cpp b/je_nourish_fusion.cpp index d8e742c..b859206 100644 --- a/je_nourish_fusion.cpp +++ b/je_nourish_fusion.cpp @@ -7,13 +7,10 @@ namespace je_nourish_fusion { class FusionDevice { public: FusionDevice(OSVR_PluginRegContext ctx, Json::Value config) { - std::cout << "Constructing fusion device" << std::endl; - - m_firstUpdate = true; osvrPose3SetIdentity(&m_state); - m_useAlign = config.isMember("alignInitialOrientation") - && config["alignInitialOrientation"].asBool(); + m_useTimestamp = config.isMember("timestamp"); + m_usePositionTimestamp = m_useTimestamp && config["timestamp"].asString().compare("position") == 0; if ((m_useOffset = config.isMember("offsetFromRotationCenter"))) { osvrVec3Zero(&m_offset); @@ -42,10 +39,10 @@ namespace je_nourish_fusion { m_orientationReader = OrientationReaderFactory::getReader(m_ctx, config["orientation"]); if (m_positionReader == NULL) { - std::cout << "Position Reader not created" << std::endl; + std::cout << "Fusion Device: Position Reader not created" << std::endl; } if (m_orientationReader == NULL) { - std::cout << "Orientation Reader not created" << std::endl; + std::cout << "Fusion Device: Orientation Reader not created" << std::endl; } m_dev->sendJsonDescriptor(je_nourish_fusion_json); @@ -53,20 +50,13 @@ namespace je_nourish_fusion { } OSVR_ReturnCode update() { - osvrClientUpdate(m_ctx); - m_positionReader->update(&m_state.translation); - m_orientationReader->update(&m_state.rotation); - + OSVR_TimeValue timeValuePosition; + OSVR_TimeValue timeValueOrientation; - if (m_useAlign) { - if (m_firstUpdate) { - m_firstUpdate = false; - setupAlign(&m_align, &m_state); - } - applyAlign(&m_align, &m_state); - } + m_positionReader->update(&m_state.translation, &timeValuePosition); + m_orientationReader->update(&m_state.rotation, &timeValueOrientation); if (m_useOffset) { Eigen::Quaterniond rotation = osvr::util::fromQuat(m_state.rotation); @@ -75,7 +65,13 @@ namespace je_nourish_fusion { translation += rotation._transformVector(osvr::util::vecMap(m_offset)); } - osvrDeviceTrackerSendPose(*m_dev, m_tracker, &m_state, 0); + if (m_useTimestamp) { + OSVR_TimeValue timeValue = m_usePositionTimestamp ? timeValuePosition : timeValueOrientation; + osvrDeviceTrackerSendPoseTimestamped(*m_dev, m_tracker, &m_state, 0, &timeValue); + } + else { + osvrDeviceTrackerSendPose(*m_dev, m_tracker, &m_state, 0); + } return OSVR_RETURN_SUCCESS; } @@ -92,13 +88,11 @@ namespace je_nourish_fusion { OSVR_TrackerDeviceInterface m_tracker; OSVR_PoseState m_state; - bool m_firstUpdate; - - bool m_useAlign; - OSVR_PoseState m_align; - bool m_useOffset; OSVR_Vec3 m_offset; + + bool m_useTimestamp; + bool m_usePositionTimestamp; }; class FusionDeviceConstructor {